class Drone2: VELOCITY = 0.2 RATE = 360.0 / 5 def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie(rw_cache='./cache') self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) # self._thread = None self._thread = _SetPointThread(self._cf) self._is_flying = False self.connected = True print('Connecting to %s' % link_uri) def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._oscillate).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.connected = False def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) self.connected = False def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) self.connected = False #########################This is From the Motion Commander Library ######### def take_off(self, height=None, velocity=VELOCITY): """ Takes off, that is starts the motors, goes straigt up and hovers. Do not call this function if you use the with keyword. Take off is done automatically when the context is created. :param height: the height (meters) to hover at. None uses the default height set when constructed. :param velocity: the velocity (meters/second) when taking off :return: """ if self._is_flying: raise Exception('Already flying') if not self._cf.is_connected(): raise Exception('Crazyflie is not connected') self._is_flying = True self._reset_position_estimator() self._thread = _SetPointThread(self._cf) self._thread.start() if height is None: height = self.default_height self.up(height, velocity) def land(self, velocity=VELOCITY): """ Go straight down and turn off the motors. Do not call this function if you use the with keyword. Landing is done automatically when the context goes out of scope. :param velocity: The velocity (meters/second) when going down :return: """ if self._is_flying: self.down(self._thread.get_height(), velocity) self._thread.stop() self._thread = None def _update_z_in_setpoint(self): self._hover_setpoint[self.ABS_Z_INDEX] = self._current_z() def _current_z(self): now = time.time() return self._z_base + self._z_velocity * (now - self._z_base_time) def __enter__(self): self.take_off() return self def __exit__(self, exc_type, exc_val, exc_tb): self.land() def left(self, distance_m, velocity=VELOCITY): """ Go left :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, distance_m, 0.0, velocity) def right(self, distance_m, velocity=VELOCITY): """ Go right :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, -distance_m, 0.0, velocity) def forward(self, distance_m, velocity=VELOCITY): """ Go forward :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(distance_m, 0.0, 0.0, velocity) def back(self, distance_m, velocity=VELOCITY): """ Go backwards :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(-distance_m, 0.0, 0.0, velocity) def up(self, distance_m, velocity=VELOCITY): """ Go up :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, distance_m, velocity) def down(self, distance_m, velocity=VELOCITY): """ Go down :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, -distance_m, velocity) def move_distance(self, distance_x_m, distance_y_m, distance_z_m, velocity=VELOCITY): """ Move in a straight line. positive X is forward positive Y is left positive Z is up :param distance_x_m: The distance to travel along the X-axis (meters) :param distance_y_m: The distance to travel along the Y-axis (meters) :param distance_z_m: The distance to travel along the Z-axis (meters) :param velocity: the velocity of the motion (meters/second) :return: """ distance = math.sqrt(distance_x_m * distance_x_m + distance_y_m * distance_y_m + distance_z_m * distance_z_m) flight_time = distance / velocity velocity_x = velocity * distance_x_m / distance velocity_y = velocity * distance_y_m / distance velocity_z = velocity * distance_z_m / distance self.start_linear_motion(velocity_x, velocity_y, velocity_z) time.sleep(flight_time) self.stop() # Velocity based primitives def start_left(self, velocity=VELOCITY): """ Start moving left. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, velocity, 0.0) def start_right(self, velocity=VELOCITY): """ Start moving right. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, -velocity, 0.0) def start_forward(self, velocity=VELOCITY): """ Start moving forward. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(velocity, 0.0, 0.0) def start_back(self, velocity=VELOCITY): """ Start moving backwards. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(-velocity, 0.0, 0.0) def start_up(self, velocity=VELOCITY): """ Start moving up. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, 0.0, velocity) def start_down(self, velocity=VELOCITY): """ Start moving down. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, 0.0, -velocity) def stop(self): """ Stop any motion and hover. :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, 0.0) def start_turn_left(self, rate=RATE): """ Start turning left. This function returns immediately. :param rate: The angular rate (degrees/second) :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, -rate) def start_turn_right(self, rate=RATE): """ Start turning right. This function returns immediately. :param rate: The angular rate (degrees/second) :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, rate) def start_circle_left(self, radius_m, velocity=VELOCITY): """ Start a circular motion to the left. This function returns immediately. :param radius_m: The radius of the circle (meters) :param velocity: The velocity of the motion (meters/second) :return: """ circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference self._set_vel_setpoint(velocity, 0.0, 0.0, -rate) def start_circle_right(self, radius_m, velocity=VELOCITY): """ Start a circular motion to the right. This function returns immediately :param radius_m: The radius of the circle (meters) :param velocity: The velocity of the motion (meters/second) :return: """ circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference self._set_vel_setpoint(velocity, 0.0, 0.0, rate) def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): """ Start a linear motion. This function returns immediately. positive X is forward positive Y is left positive Z is up :param velocity_x_m: The velocity along the X-axis (meters/second) :param velocity_y_m: The velocity along the Y-axis (meters/second) :param velocity_z_m: The velocity along the Z-axis (meters/second) :return: """ self._set_vel_setpoint(velocity_x_m, velocity_y_m, velocity_z_m, 0.0) def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw): if not self._is_flying: raise Exception('Can not move on the ground. Take off first!') self._thread.set_vel_setpoint(velocity_x, velocity_y, velocity_z, rate_yaw) def _reset_position_estimator(self): self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) ####### Actually getting it to oscillate ########################## def _oscillate(self): self.take_off(0.35, 0.6) #self.move_distance(0,0,0.25,0.6) amplitude = 0.25 peak = 2 * amplitude global t while True: if keyboard.is_pressed('w'): oscillate = True while oscillate == True: self.move_distance(0, 0, peak, v_inp2) t = t + tau2 / 2 disp = np.exp(-z2 * wn2 * t) * amplitude peak = amplitude + disp amplitude = disp print(peak) time.sleep(0.2) # self.start_circle_right(0.43,0.3) self.move_distance(0, 0, -peak, v_inp2) self.start_circle_right(0.43, 0.3) t = t + tau2 / 2 disp = np.exp(-z2 * wn2 * t) * amplitude peak = amplitude + disp amplitude = disp print(peak) time.sleep(0.2) # self.move_distance(0,0,-peak,v_inp2) # while True: if keyboard.is_pressed('p') or peak < 2.18 * 10**(-7): oscillate = False # break break while True: if keyboard.is_pressed('s'): oscillate == False self.land(velocity=0.6) break self._cf.close_link()
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none """ cflib.crtp.init_drivers() #self.FD = open("logFile.txt", "w") self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() if (len(self.available) > 0): if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: self.UAV.open_link(self.available[0][0]) while (self.UAV.is_connected() == False): time.sleep(0.1) self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) #self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') #self.UAVLogConfig.add_variable('stateEstimate.x', 'float') #self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') self.UAVLogConfig.add_variable('pm.extCurr', 'float') self.UAVLogConfig.add_variable('pwm.m1_pwm', 'float') #self.UAVLogConfig.add_variable('baro.pressure', 'float') #self.UAVLogConfig.add_variable('extrx.thrust', 'float') #Add more variables here for logging as desired self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") self._startTime = time.time() #For testing purposes self._trialRun = 0 #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none """ self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distance - a floating point value distance in meters velocity - a floating point value velocity in meters per second Outputs: none """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 seclond, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: Process a data packet received from the UAV Inputs: ident - data - logconfig - Outputs: None Description: A user should never call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False def appendToLogFile(self): """ Function: appendToLogFile Purpose: append log variables to log file 'log.txt.' Inputs: none Outputs: none Description: """ #retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): current = self._recentDataPacket["pm.chargeCurrent"] extcurrent = self._recentDataPacket["pm.extCurr"] voltage = self._recentDataPacket["pm.vbat"] bat_level = self._recentDataPacket["pm.batteryLevel"] height = self._recentDataPacket["stateEstimate.z"] #x = self._recentDataPacket["stateEstimate.y"] #y = self._recentDataPacket["stateEstimate.x"] m1_pwm = self._recentDataPacket["pwm.m1_pwm"] #extrx_thrust = self._recentDataPacket["extrx.thrust"] #baro_pressure = self._recentDataPacket["baro.pressure"] with open('log2.txt', 'a') as file: #print(batlevel) file.write(str(datetime.now()) + '\n') file.write('bat_voltage: ' + str(voltage) + '\n') file.write('bat_level: ' + str(bat_level) + '\n') file.write('crg_current: ' + str(current) + '\n') file.write('ext_current: ' + str(extcurrent) + '\n') file.write('height: ' + str(height) + '\n') #file.write('x: ' + str(x) + '\n') #file.write('y: ' + str(y) + '\n') file.write('m1_pwm: ' + str(m1_pwm) + '\n')
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none Description: An initializer function that finds a Crazyflie, can be particular target or not, and sets up all necessary data values for logging data values from the Crazyflie. """ #Enable the CrazyRadio PA drivers to communicate with the UAV. cflib.crtp.init_drivers() self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() #If there are Crazyflies available, connect to one of them if (len(self.available) > 0): #If there is a specific target, parse through the returned array to locate specific Crazyflie if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): #If found, open a link to the Crazyflie self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: #If there is not a specific target, simply connect to the first Crazyflie available self.UAV.open_link(self.available[0][0]) #While the Crazyflie is not connected, wait until the connection occurs to allow for Motion Commander initialization while (self.UAV.is_connected() == False): time.sleep(0.1) #Pass the connected Crazyflie to the Motion Commander class self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('stateEstimate.x', 'float') self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') #Add more variables here for logging as desired #Add the configured logger to the Crazyflie to begin grabbing data packets self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none Description: See purpose. """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none Description: A wrapper function that calls the Crazyflie Motion Commander take_off function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none Description: A function that calls the Crazyflie Motion Commander Land function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = False self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distanceX - a floating point value that represents the distance to move the in the X-dimension, measured in meters. distanceY - a floating point value that represents the distance to move the in the Y-dimension, measured in meters. distanceZ - a floating point value that represents the distance to move the in the Z-dimension, measured in meters. velocity - a floating point value that represents the velocity of the UAV during movement, measured in meters per second Outputs: none Description: See purpose. """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none Description: Currently this function is not in use as rotating the Crazyflie regularly introduces positional errors. In particular, the Crazyflie will rotate on a motor, not the center of the drone. """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 second, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from an IOStream Inputs: none Outputs: retVal - a floating point value that represents the battery voltage of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream. Inputs: none Outputs: retVal - a floating point value that represents the onboard height detection of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV charge current from a IOStream Inputs: none Outputs: retVal - a floating point value that represents the charge current of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: A callback function to process a data packet received from the UAV Inputs: ident - identifier of the UAV data - data from the UAV logconfig - log configuration from the UAV Outputs: None Description: A user should NEVER call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False
class CrazyDrone(Drone): def __init__(self, link_uri): super().__init__() cache = "./cache" if getattr(sys, 'frozen', False): cache = sys._MEIPASS + os.path.sep + "cache" self._cf = Crazyflie(rw_cache=cache) self.motion_commander = None self.multiranger = None # maximum speeds self.max_vert_speed = 1 self.max_horiz_speed = 1 self.max_rotation_speed = 90 self.logger = None # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) print('Connecting to %s' % link_uri) # Try to connect to the Crazyflie self._cf.open_link(link_uri) # Variable used to keep main loop occupied until disconnect self.is_connected = True def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) self.connection.emit("progress") # The definition of the logconfig can be made before connecting self.logger = LogConfig("Battery", 1000) # delay self.logger.add_variable("pm.vbat", "float") try: self._cf.log.add_config(self.logger) self.logger.data_received_cb.add_callback( lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat']))) # self.logger.error_cb.add_callback(lambda: print('error')) self.logger.start() except KeyError as e: print(e) self.connection.emit("on") self.motion_commander = MotionCommander(self._cf, 0.5) self.multiranger = Multiranger(self._cf, rate_ms=50) self.multiranger.start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False self.connection.emit("off") def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) self.connection.emit("off") def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) self.is_connected = False self.connection.emit("off") def take_off(self): if self._cf.is_connected( ) and self.motion_commander and not self.motion_commander._is_flying: self.motion_commander.take_off() self.is_flying_signal.emit(True) def land(self): if self._cf.is_connected( ) and self.motion_commander and self.motion_commander._is_flying: self.motion_commander.land() self.is_flying_signal.emit(False) def stop(self): if not (self.logger is None): self.logger.stop() if self.motion_commander: self.motion_commander.land() if self.multiranger: self.multiranger.stop() self._cf.close_link() def is_flying(self): if self._cf.is_connected() and self.motion_commander: return self.motion_commander._is_flying return False def process_motion(self, _up, _rotate, _front, _right): if self.motion_commander: # WARNING FOR CRAZYFLY # positive X is forward, # positive Y is left # positive Z is up velocity_z = _up * self.max_vert_speed velocity_yaw = _rotate * self.max_rotation_speed velocity_x = _front * self.max_horiz_speed velocity_y = -_right * self.max_horiz_speed # print("PRE", velocity_x, velocity_y, velocity_z, velocity_yaw) # protect against collision by reducing speed depending on distance ranger = self.multiranger if ranger.front and ranger.front < ANTI_COLLISION_DISTANCE and velocity_x > 0: velocity_x = velocity_x * ( ranger.front - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_x = max(0, velocity_x) if ranger.back and ranger.back < ANTI_COLLISION_DISTANCE and velocity_x < 0: velocity_x = velocity_x * ( ranger.back - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_x = min(0, velocity_x) if ranger.left and ranger.left < ANTI_COLLISION_DISTANCE and velocity_y > 0: velocity_y = velocity_y * ( ranger.left - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_y = max(0, velocity_y) if ranger.right and ranger.right < ANTI_COLLISION_DISTANCE and velocity_y < 0: velocity_y = velocity_y * ( ranger.left - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_y = min(0, velocity_y) if ranger.up and ranger.up < ANTI_COLLISION_DISTANCE and velocity_z > 0: velocity_z = velocity_z * (ranger.up - ANTI_COLLISION_MIN_DIST ) / ANTI_COLLISION_DISTANCE velocity_z = max(0, velocity_z) # print("POST", velocity_x, velocity_y, velocity_z, velocity_yaw) if self.motion_commander._is_flying: self.motion_commander._set_vel_setpoint( velocity_x, velocity_y, velocity_z, velocity_yaw)
class MultiSpring: VELOCITY = 0.2 RATE = 360.0 / 5 # m = float(input("Input a mass value")) # k = float(input("Input a spring constant")) # c = float(input("Input a damping coeficient")) # # z = c/(2*np.sqrt(m*k)) # Damping Ratio # wn = np.sqrt(k/m) # natural Freq. # w = wn*np.sqrt(1-z**2) # Damped Nat. Freq. # f = w/(2*np.pi) # Frequncy (hz) # tau = 1/f # Period (s) # t = 0 # initial time (s) # # sleep = (0.2*0.758)/f # amplitude = 0.25 # v = (f/0.758)*(amplitude/0.25) # # v_inp = 1/((1/v)+((sleep - 0.2)/(2*amplitude)))#-(1.94*(m/k)-1.1*c) # print(sleep) # print(v) # print(w) # print(wn) # print(z) # print(tau) # print(v_inp) # # peak = 2*amplitude def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie(rw_cache='./cache') self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) # self._thread = None self._thread = _SetPointThread(self._cf) self._is_flying = False self.connected = True print('Connecting to %s' % link_uri) def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._oscillate).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.connected = False def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) self.connected = False def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) self.connected = False #########################This is From the Motion Commander Library ######### def take_off(self, height=None, velocity=VELOCITY): """ Takes off, that is starts the motors, goes straigt up and hovers. Do not call this function if you use the with keyword. Take off is done automatically when the context is created. :param height: the height (meters) to hover at. None uses the default height set when constructed. :param velocity: the velocity (meters/second) when taking off :return: """ if self._is_flying: raise Exception('Already flying') if not self._cf.is_connected(): raise Exception('Crazyflie is not connected') self._is_flying = True self._reset_position_estimator() self._thread = _SetPointThread(self._cf) self._thread.start() if height is None: height = self.default_height self.up(height, velocity) def land(self, velocity=VELOCITY): """ Go straight down and turn off the motors. Do not call this function if you use the with keyword. Landing is done automatically when the context goes out of scope. :param velocity: The velocity (meters/second) when going down :return: """ if self._is_flying: self.down(self._thread.get_height(), velocity) self._thread.stop() self._thread = None self._cf.commander.send_stop_setpoint() self._is_flying = False def __enter__(self): self.take_off() return self def __exit__(self, exc_type, exc_val, exc_tb): self.land() def left(self, distance_m, velocity=VELOCITY): """ Go left :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, distance_m, 0.0, velocity) def right(self, distance_m, velocity=VELOCITY): """ Go right :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, -distance_m, 0.0, velocity) def forward(self, distance_m, velocity=VELOCITY): """ Go forward :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(distance_m, 0.0, 0.0, velocity) def back(self, distance_m, velocity=VELOCITY): """ Go backwards :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(-distance_m, 0.0, 0.0, velocity) def up(self, distance_m, velocity=VELOCITY): """ Go up :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, distance_m, velocity) def down(self, distance_m, velocity=VELOCITY): """ Go down :param distance_m: the distance to travel (meters) :param velocity: the velocity of the motion (meters/second) :return: """ self.move_distance(0.0, 0.0, -distance_m, velocity) def turn_left(self, angle_degrees, rate=RATE): """ Turn to the left, staying on the spot :param angle_degrees: How far to turn (degrees) :param rate: The trurning speed (degrees/second) :return: """ flight_time = angle_degrees / rate self.start_turn_left(rate) time.sleep(flight_time) self.stop() def turn_right(self, angle_degrees, rate=RATE): """ Turn to the right, staying on the spot :param angle_degrees: How far to turn (degrees) :param rate: The trurning speed (degrees/second) :return: """ flight_time = angle_degrees / rate self.start_turn_right(rate) time.sleep(flight_time) self.stop() def circle_left(self, radius_m, velocity=VELOCITY, angle_degrees=360.0): """ Go in circle, counter clock wise :param radius_m: The radius of the circle (meters) :param velocity: The velocity along the circle (meters/second) :param angle_degrees: How far to go in the circle (degrees) :return: """ distance = 2 * radius_m * math.pi * angle_degrees / 360.0 flight_time = distance / velocity self.start_circle_left(radius_m, velocity) time.sleep(flight_time) self.stop() def circle_right(self, radius_m, velocity=VELOCITY, angle_degrees=360.0): """ Go in circle, clock wise :param radius_m: The radius of the circle (meters) :param velocity: The velocity along the circle (meters/second) :param angle_degrees: How far to go in the circle (degrees) :return: """ distance = 2 * radius_m * math.pi * angle_degrees / 360.0 flight_time = distance / velocity self.start_circle_right(radius_m, velocity) time.sleep(flight_time) self.stop() def move_distance(self, distance_x_m, distance_y_m, distance_z_m, velocity=VELOCITY): """ Move in a straight line. positive X is forward positive Y is left positive Z is up :param distance_x_m: The distance to travel along the X-axis (meters) :param distance_y_m: The distance to travel along the Y-axis (meters) :param distance_z_m: The distance to travel along the Z-axis (meters) :param velocity: the velocity of the motion (meters/second) :return: """ distance = math.sqrt(distance_x_m * distance_x_m + distance_y_m * distance_y_m + distance_z_m * distance_z_m) flight_time = distance / velocity velocity_x = velocity * distance_x_m / distance velocity_y = velocity * distance_y_m / distance velocity_z = velocity * distance_z_m / distance self.start_linear_motion(velocity_x, velocity_y, velocity_z) time.sleep(flight_time) self.stop() # Velocity based primitives def start_left(self, velocity=VELOCITY): """ Start moving left. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, velocity, 0.0) def start_right(self, velocity=VELOCITY): """ Start moving right. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, -velocity, 0.0) def start_forward(self, velocity=VELOCITY): """ Start moving forward. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(velocity, 0.0, 0.0) def start_back(self, velocity=VELOCITY): """ Start moving backwards. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(-velocity, 0.0, 0.0) def start_up(self, velocity=VELOCITY): """ Start moving up. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, 0.0, velocity) def start_down(self, velocity=VELOCITY): """ Start moving down. This function returns immediately. :param velocity: The velocity of the motion (meters/second) :return: """ self.start_linear_motion(0.0, 0.0, -velocity) def stop(self): """ Stop any motion and hover. :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, 0.0) def start_turn_left(self, rate=RATE): """ Start turning left. This function returns immediately. :param rate: The angular rate (degrees/second) :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, -rate) def start_turn_right(self, rate=RATE): """ Start turning right. This function returns immediately. :param rate: The angular rate (degrees/second) :return: """ self._set_vel_setpoint(0.0, 0.0, 0.0, rate) def start_circle_left(self, radius_m, velocity=VELOCITY): """ Start a circular motion to the left. This function returns immediately. :param radius_m: The radius of the circle (meters) :param velocity: The velocity of the motion (meters/second) :return: """ circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference self._set_vel_setpoint(velocity, 0.0, 0.0, -rate) def start_circle_right(self, radius_m, velocity=VELOCITY): """ Start a circular motion to the right. This function returns immediately :param radius_m: The radius of the circle (meters) :param velocity: The velocity of the motion (meters/second) :return: """ circumference = 2 * radius_m * math.pi rate = 360.0 * velocity / circumference self._set_vel_setpoint(velocity, 0.0, 0.0, rate) def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m): """ Start a linear motion. This function returns immediately. positive X is forward positive Y is left positive Z is up :param velocity_x_m: The velocity along the X-axis (meters/second) :param velocity_y_m: The velocity along the Y-axis (meters/second) :param velocity_z_m: The velocity along the Z-axis (meters/second) :return: """ self._set_vel_setpoint(velocity_x_m, velocity_y_m, velocity_z_m, 0.0) def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw): if not self._is_flying: raise Exception('Can not move on the ground. Take off first!') self._thread.set_vel_setpoint(velocity_x, velocity_y, velocity_z, rate_yaw) def _reset_position_estimator(self): self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) ####### Actually getting it to oscillate ########################## def _oscillate(self): self.take_off(0.35, 0.6) global amplitude global peak global t while True: if keyboard.is_pressed('w'): oscillate = True while oscillate == True: self.move_distance(0, 0, peak, v_inp) t = t + tau / 2 disp = np.exp(-z * wn * t) * amplitude peak = amplitude + disp amplitude = disp print(peak) time.sleep(0.2) self.move_distance(0, 0, -peak, v_inp) t = t + tau / 2 disp = np.exp(-z * wn * t) * amplitude peak = amplitude + disp amplitude = disp print(peak) time.sleep(0.2) # while True: if keyboard.is_pressed('p') or peak < 2.18 * 10**(-7): oscillate = False # break break while True: if keyboard.is_pressed('s'): oscillate == False self.land(velocity=0.6) break self._cf.close_link()
class LogFlight(): def __init__(self, args): self.args = args self.optitrack_enabled = False self.console_dump_enabled = False cflib.crtp.init_drivers(enable_debug_driver=False) self._cf = Crazyflie(rw_cache="./cache") self._jr = JoystickReader(do_device_discovery=False) # Set flight mode if self.args["trajectory"] is None or \ self.args["trajectory"][0] == "none": self.mode = Mode.DONT_FLY print("Mode set to [DONT_FLY]") elif self.args["trajectory"][0] == "manual": self.mode = Mode.MANUAL print("Mode set to [MANUAL]") elif self.args["safetypilot"]: self.mode = Mode.MODE_SWITCH print("Mode set to [MODE_SWITCH]") else: self.mode = Mode.AUTO print("Mode set to [AUTO]") # Setup for specified mode if self.mode == Mode.AUTO: self.is_in_manual_control = False # Make sure drone is setup to perform autonomous flight if args["uwb"] == "none": assert args["optitrack"] == "state", "OptiTrack state needed in absence of UWB" assert args["estimator"] == "kalman", "OptiTrack state needs Kalman estimator" elif self.mode == Mode.DONT_FLY: self.is_in_manual_control = False else: # Check if controller is connected assert self.controller_connected(), "No controller detected." self.setup_controller(map="flappy") self.is_in_manual_control = True # Setup the logging framework self.setup_logger() # Setup optitrack if required if not args["optitrack"] == "none": self.setup_optitrack() def get_filename(self): # create default fileroot if not provided if self.args["fileroot"] is None: date = datetime.today().strftime(r"%Y_%m_%d") self.args["fileroot"] = "data/" + date fileroot = self.args["fileroot"] # create default filename if not provided if self.args["filename"] is None: estimator = self.args["estimator"] uwb = self.args["uwb"] optitrack = self.args["optitrack"] trajectory = self.args["trajectory"] date = datetime.today().strftime(r"%Y-%m-%d+%H:%M:%S") traj = '_'.join(trajectory) if optitrack == "logging": options = "{}+{}+optitracklog+{}".format(estimator, uwb, traj) elif optitrack == "state": options = "{}+{}+optitrackstate+{}".format(estimator, uwb, traj) else: options = "{}+{}+{}".format(estimator, uwb, traj) name = "{}+{}.csv".format(date, options) fname = os.path.normpath(os.path.join(os.getcwd(), fileroot, name)) else: # make sure provided filename is unique if self.args["filename"].endswith(".csv"): name = self.args["filename"][:-4] else: name = self.args["filename"] new_name = name + ".csv" fname = os.path.normpath(os.path.join( os.getcwd(), fileroot, new_name)) i = 0 while os.path.isfile(fname): i = i + 1 new_name = name + "_" + str(i) + ".csv" fname = os.path.normpath(os.path.join( os.getcwd(), fileroot, new_name)) return fname def setup_logger(self): # Create filename from options and date self.log_file = self.get_filename() # Create directory if not there Path(self.args["fileroot"]).mkdir(exist_ok=True) print("Log location: {}".format(self.log_file)) # Logger setup logconfig = self.args["logconfig"] self.flogger = FileLogger(self._cf, logconfig, self.log_file) self.flogger.enableAllConfigs() def setup_optitrack(self): self.ot_id = self.args["optitrack_id"] self.ot_position = np.zeros(3) self.ot_attitude = np.zeros(3) self.ot_quaternion = np.zeros(4) self.filtered_pos = np.zeros(3) self.ot_filter_sos = scipy.signal.butter(N=4, Wn=0.1, btype='low', analog=False, output='sos') self.pos_filter_zi = [scipy.signal.sosfilt_zi(self.ot_filter_sos), scipy.signal.sosfilt_zi(self.ot_filter_sos), scipy.signal.sosfilt_zi(self.ot_filter_sos)] # Streaming client in separate thread streaming_client = NatNetClient() streaming_client.newFrameListener = self.ot_receive_new_frame streaming_client.rigidBodyListener = self.ot_receive_rigidbody_frame streaming_client.run() self.optitrack_enabled = True print("OptiTrack streaming client started") # TODO: do we need to return StreamingClient? def reset_estimator(self): # Kalman if self.args["estimator"] == "kalman": self._cf.param.set_value("kalman.resetEstimation", "1") time.sleep(1) self._cf.param.set_value("kalman.resetEstimation", "0") # Complementary (needs changes to firmware) if self.args["estimator"] == "complementary": try: self._cf.param.set_value("complementaryFilter.reset", "1") time.sleep(1) self._cf.param.set_value("complementaryFilter.reset", "0") except: pass def ot_receive_new_frame(self, *args, **kwargs): pass def ot_receive_rigidbody_frame(self, id, position, rotation): # Check ID if id in self.ot_id: # get optitrack data in crazyflie global frame pos_in_cf_frame = util.ot2control(position) att_in_cf_frame = util.quat2euler(rotation) quat_in_cf_frame = util.ot2control_quat(rotation) idx = self.ot_id.index(id) if idx==0: # main drone ot_dict = { "otX0": pos_in_cf_frame[0], "otY0": pos_in_cf_frame[1], "otZ0": pos_in_cf_frame[2], "otRoll0": att_in_cf_frame[0], "otPitch0": att_in_cf_frame[1], "otYaw0": att_in_cf_frame[2] } self.ot_position = pos_in_cf_frame self.ot_attitude = att_in_cf_frame self.ot_quaternion = quat_in_cf_frame self.flogger.registerData("ot0", ot_dict) (self.filtered_pos[0], self.pos_filter_zi[0]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[0]], zi=self.pos_filter_zi[0] ) (self.filtered_pos[1], self.pos_filter_zi[1]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[1]], zi=self.pos_filter_zi[1] ) (self.filtered_pos[2], self.pos_filter_zi[2]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[2]], zi=self.pos_filter_zi[2] ) elif idx==1: ot_dict = { "otX1": pos_in_cf_frame[0], "otY1": pos_in_cf_frame[1], "otZ1": pos_in_cf_frame[2], "otRoll1": att_in_cf_frame[0], "otPitch1": att_in_cf_frame[1], "otYaw1": att_in_cf_frame[2] } self.flogger.registerData("ot1", ot_dict) def do_taskdump(self): self._cf.param.set_value("system.taskDump", "1") def process_taskdump(self, console_log): file = self.get_filename() # Dataframe placeholders label_data, load_data, stack_data = [], [], [] # Get headers headers = [] for i, line in enumerate(console_log): if "Task dump" in line: headers.append(i) # None indicates the end of the list headers.append(None) # Get one task dump for i in range(len(headers) - 1): dump = console_log[headers[i] + 2 : headers[i + 1]] # Process strings: strip \n, \t, spaces, SYSLOAD: loads, stacks, labels = [], [], [] for line in dump: entries = line.strip("SYSLOAD: ").split("\t") loads.append(entries[0].strip()) # no sep means strip all space, \n, \t stacks.append(entries[1].strip()) labels.append(entries[2].strip()) # Store labels if not label_data: label_data = labels # Append to placeholders load_data.append(loads) stack_data.append(stacks) # Check if we have data at all if headers[0] is not None and label_data: # Put in dataframe load_data = pd.DataFrame(load_data, columns=label_data) stack_data = pd.DataFrame(stack_data, columns=label_data) # Save dataframes load_data.to_csv(file.strip(".csv") + "+load.csv", sep=",", index=False) stack_data.to_csv(file.strip(".csv") + "+stackleft.csv", sep=",", index=False) else: print("No task dump data found") def controller_connected(self): """ Return True if a controller is connected """ return len(self._jr.available_devices()) > 0 def setup_controller(self, map="PS3_Mode_1"): devs = [] for d in self._jr.available_devices(): devs.append(d.name) if len(devs)==1: input_device = 0 else: print("Multiple controllers detected:") for i, dev in enumerate(devs): print(" - Controller #{}: {}".format(i, dev)) input_device = int(input("Select controller: ")) if not input_device in range(len(devs)): raise ValueError self._jr.start_input(devs[input_device]) self._jr.set_input_map(devs[input_device], map) def connect_crazyflie(self, uri): """Connect to a Crazyflie on the given link uri""" # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) if self.mode == Mode.AUTO or self.mode == Mode.DONT_FLY: self._cf.open_link(uri) else: # Add callbacks for manual control self._cf.param.add_update_callback( group="imu_sensors", name="AK8963", cb=( lambda name, found: self._jr.set_alt_hold_available( eval(found)))) # self._jr.assisted_control_updated.add_callback( # lambda enabled: self._cf.param.set_value("flightmode.althold", # enabled)) self._cf.open_link(uri) self._jr.input_updated.add_callback(self.controller_input_cb) if self.mode == Mode.MODE_SWITCH: self._jr.alt1_updated.add_callback(self.mode_switch_cb) def _connected(self, link): """This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print("Connected to %s" % link) # set estimator if args["estimator"]=="kalman": self._cf.param.set_value("stabilizer.estimator", "2") self.flogger.start() print("logging started") def _connection_failed(self, link_uri, msg): print("Connection to %s failed: %s" % (link_uri, msg)) self.flogger.is_connected = False def _connection_lost(self, link_uri, msg): print("Connection to %s lost: %s" % (link_uri, msg)) self.flogger.is_connected = False def _disconnected(self, link_uri): print("Disconnected from %s" % link_uri) self.flogger.is_connected = False def ready_to_fly(self): # Wait for connection timeout = 10 while not self._cf.is_connected(): print("Waiting for Crazyflie connection...") time.sleep(2) timeout -= 1 if timeout<=0: return False # Wait for optitrack if self.optitrack_enabled: while (self.ot_position == 0).any(): print("Waiting for OptiTrack fix...") time.sleep(2) timeout -= 1 if timeout <= 0: return False print("OptiTrack fix acquired") print("Reset Estimator...") self.reset_estimator() time.sleep(2) # wait for kalman to stabilize return True def start_flight(self): if self.ready_to_fly(): if self.mode == Mode.MANUAL: print("Manual Flight - Ready to fly") self.manual_flight() elif self.mode == Mode.DONT_FLY: print("Ready to not fly") try: while True: pass except KeyboardInterrupt: print("Flight stopped") else: # Build trajectory setpoints = self.build_trajectory(self.args["trajectory"], self.args["space"]) # Do flight if self.mode == Mode.AUTO: print("Autonomous Flight - Starting flight") self.follow_setpoints(self._cf, setpoints, self.args["optitrack"]) print("Flight complete.") else: print("Ready to fly") self.manual_flight() print("Starting Trajectory") self.follow_setpoints(self._cf, setpoints, self.args["optitrack"]) else: print("Timeout while waiting for flight ready.") def controller_input_cb(self, *data): # only forward control in manual mode if self.is_in_manual_control: self._cf.commander.send_setpoint(*data) def mode_switch_cb(self, auto_mode): if auto_mode: print("Switching autonomous flight") self.is_in_manual_control = False else: print("Switching to manual control") self.is_in_manual_control = True def manual_flight(self): self.is_in_manual_control = True while(self.is_in_manual_control): if self.args["optitrack"]=="state": # self._cf.extpos.send_extpos( # self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2] # ) self._cf.extpos.send_extpos( self.ot_position[0], self.ot_position[1], self.ot_position[2] ) # self._cf.extpos.send_extpose( # self.ot_position[0], self.ot_position[1], self.ot_position[2], # self.ot_quaternion[0], self.ot_quaternion[1], self.ot_quaternion[2], self.ot_quaternion[3] # ) time.sleep(0.01) def build_trajectory(self, trajectories, space): # Load yaml file with space specification with open(space, "r") as f: space = yaml.full_load(f) home = space["home"] ranges = space["range"] # Account for height offset altitude = home["z"] + ranges["z"] side_length = min([ranges["x"], ranges["y"]]) * 2 radius = min([ranges["x"], ranges["y"]]) x_bound = [home["x"] - ranges["x"], home["x"] + ranges["x"]] y_bound = [home["y"] - ranges["y"], home["y"] + ranges["y"]] # Build trajectory # Takeoff setpoints = takeoff(home["x"], home["y"], altitude, 0.0) for trajectory in trajectories: # If nothing, only nothing if trajectory == "nothing": setpoints = None return setpoints elif trajectory == "hover": setpoints += hover(home["x"], home["y"], altitude) elif trajectory == "hover_fw": setpoints += hover_fw(home["x"], home["y"], altitude) elif trajectory == "square": setpoints += square(home["x"], home["y"], side_length, altitude) elif trajectory == "square_fw": setpoints += square_fw(home["x"], home["y"], side_length, altitude) elif trajectory == "octagon": setpoints += octagon(home["x"], home["y"], radius, altitude) elif trajectory == "triangle": setpoints += triangle(home["x"], home["y"], radius, altitude) elif trajectory == "hourglass": setpoints += hourglass(home["x"], home["y"], side_length, altitude) elif trajectory == "random": setpoints += randoms(home["x"], home["y"], x_bound, y_bound, altitude) elif trajectory == "scan": setpoints += scan(home["x"], home["y"], x_bound, y_bound, altitude) else: raise ValueError("{} is an unknown trajectory".format(trajectory)) # Add landing setpoints += landing(home["x"], home["y"], altitude, 0.0) return setpoints def follow_setpoints(self, cf, setpoints, optitrack): # Counter for task dump logging time_since_dump = 0.0 # Start try: print("Flight started") # Do nothing, just sit on the ground if setpoints is None: while True: time.sleep(0.05) time_since_dump += 0.05 # Task dump if time_since_dump > 2: print("Do task dump") self.do_taskdump() time_since_dump = 0.0 # Do actual flight else: for i, point in enumerate(setpoints): print("Next setpoint: {}".format(point)) # Compute time based on distance # Take-off if i == 0: distance = point[2] # No take-off else: distance = np.sqrt( ((np.array(point[:3]) - np.array(setpoints[i - 1][:3])) ** 2).sum() ) # If zero distance, at least some wait time if distance == 0.0: wait = 5 else: wait = distance * 2 # Send position and wait time_passed = 0.0 while time_passed < wait: if self.is_in_manual_control: self.manual_flight() # If we use OptiTrack for control, send position to Crazyflie if optitrack == "state": cf.extpos.send_extpos( self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2] ) cf.commander.send_position_setpoint(*point) time.sleep(0.05) time_passed += 0.05 time_since_dump += 0.05 # Task dump # if time_since_dump > 2: # print("Do task dump") # self.do_taskdump() # time_since_dump = 0.0 # Finished cf.commander.send_stop_setpoint() # Prematurely break off flight / quit doing nothing except KeyboardInterrupt: if setpoints is None: print("Quit doing nothing!") else: print("Emergency landing!") wait = setpoints[i][2] * 2 cf.commander.send_position_setpoint(setpoints[i][0], setpoints[i][1], 0.0, 0.0) time.sleep(wait) cf.commander.send_stop_setpoint() def setup_console_dump(self): # Console dump file self.console_log = [] console = Console(self._cf) console.receivedChar.add_callback(self._console_cb) self.console_dump_enabled = True def _console_cb(self, text): # print(text) self.console_log.append(text) def end(self): self._cf.close_link() # Process task dumps # TODO: add timestamps / ticks (like logging) to this if self.console_dump_enabled: self.process_taskdump(self.console_log)