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)
