class Main: """ Class is required so that methods can access the object fields. """ def __init__(self): """ Connect to Crazyflie, initialize drivers and set up callback. The callback takes care of logging the accelerometer values. """ self.crazyflie = Crazyflie() cflib.crtp.init_drivers() self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) self.crazyflie.open_link("radio://0/8/250K") def connectSetupFinished(self, linkURI): # Set accelerometer logging config motor_log_conf = LogConfig("motor", 10) motor_log_conf.addVariable(LogVariable("motor.m4", "int32_t")) motor_log_conf.addVariable(LogVariable("motor.m1", "int32_t")) motor_log_conf.addVariable(LogVariable("motor.m2", "int32_t")) motor_log_conf.addVariable(LogVariable("motor.m3", "int32_t")) # Now that the connection is established, start logging self.motor_log = self.crazyflie.log.create_log_packet(motor_log_conf) if self.motor_log is not None: self.motor_log.dataReceived.add_callback(self.log_motor_data) self.motor_log.start() else: print("motor.m1/m2/m3/m4 not found in log TOC") self.crazyflie.close_link() def log_motor_data(self,data): thrust_mult = 3 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if (thrust >= 50000): time.sleep(.01) thrust_mult = -1 thrust = thrust + (thrust_step * thrust_mult) self.crazyflie.commander.send_setpoint(0,0,0,0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def log_motor_data(self, data): logging.info("Motors: m1=%.2f, m2=%.2f, m3=%.2f, m4=%.2f" % (data["motor.m1"], data["motor.m2"], data["motor.m3"], data["motor.m4"]))
class Main: def __init__(self, uri): self.crazyflie = Crazyflie() self.uri = uri cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link(uri) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self.pulse_command).start() def pulse_command(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self.crazyflie.close_link()
class Main: def __init__(self): self._stop = 0 self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): lg = LogConfig("Magnetometer", 100) lg.addVariable(LogVariable("mag.x", "int16_t")) lg.addVariable(LogVariable("mag.y", "int16_t")) lg.addVariable(LogVariable("mag.z", "int16_t")) log = self.crazyflie.log.create_log_packet(lg) if log is not None: log.dataReceived.add_callback(self.magData) log.start() else: print "Magnetometer not found in log TOC" # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() Thread(target=self.input_loop).start() def stop(self): # Exit command received # Set thrust to zero to make sure the motors turn off NOW # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1; # Exit the main loop time.sleep(1) self.crazyflie.close_link() # This errors out for some reason. Bad libusb? sys.exit(0) def input_loop(self): command = raw_input("") self.stop() def pulse_command(self): while 1: self.crazyflie.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) # Exit if the parent told us to if self._stop==1: return def magData(self, data): x, y, z = data['mag.x'], data['mag.y'], data['mag.z'] try: print x, y, z sys.stdout.flush() except: self.stop()
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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) 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) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 20000#20000 thrustmax = 45000#25000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: print "Thrust: "+str(round(thrust,2)) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= thrustmax: thrust_mult = -1 thrust += thrust_step * thrust_mult self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self._cf.close_link()
class AltHold: def __init__(self, link_uri): self._cf = Crazyflie() 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) print("Connecting to %s" % link_uri) def _connected(self, link_uri): Thread(target=self._ramp_motors).start() def _connection_failed(self, link_uri, msg): print("Connection to %s failed: %s" % (link_uri, msg)) def _connection_lost(self, link_uri, msg): print("Connection to %s lost: %s" % (link_uri, msg)) def _disconnected(self, link_uri): print("Disconnected from %s" % link_uri) def _ramp_motors(self): thrust = 36000 #Works at JoBa pitch = 0 roll = 0 yawrate = 10 self._cf.commander.send_setpoint(0, 0, 0, 0) print("Constant thrust") self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.75) print("Turning on altitude hold") self._cf.param.set_value("flightmode.althold", "True") start_time = time.time() timeC =time.time() - start_time while timeC<3: #wait for 3 seconds self._cf.commander.send_setpoint(0, 0, 0, 32767) time.sleep(0.1) print("hovering!") timeC =time.time() - start_time print("Closing link, time taken was: ") print timeC #close link and execute landing self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self._cf.close_link()
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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) 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) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 #m1list = [20000,25000,30000,35000,40000,45000,50000,55000,60000] m1list = [20000] m2 = 0 m3 = 0 m4 = 0 for m1 in m1list: for i in range(2000): time.sleep(0.01) self._cf.commander.send_setpoint(0, 45, 0, m1) self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(5) self._cf.close_link()
class Controller: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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.""" print "Connected." #self._quadcopter = Quadcopter (self._cf) # The older, harder-to-use code self._quadcopter = QuadcopterAltHold (self._cf) # Altitude hold mode! Thread(target=self._run).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) 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) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _quality_updated(self, quality): if quality < 30: print "Low link quality: %s", quality def _run(self): print "Running..." current_time = 0 self._cf.commander.send_setpoint (0,0,0,0); while current_time <= 5: print current_time self._quadcopter._update(self._cf) time.sleep(0.1) current_time += 0.1 self._cf.close_link() print "Done."
class Main: def __init__(self, uri): self.crazyflie = Crazyflie() self.uri = uri cflib.crtp.init_drivers(enable_debug_driver=False) # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link(uri) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) self.crazyflie.disconnected.add_callback(self._disconnected) self.crazyflie.connection_failed.add_callback(self._connection_failed) self.crazyflie.connection_lost.add_callback(self._connection_lost) def connectSetupFinished(self, linkURI): # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self.pulse_command).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri) def pulse_command(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 # Unlock startup thrust protection print("unlock crazyflie") self.crazyflie.commander.send_setpoint(0, 0, 0, 0) while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) print("command Ended") self.crazyflie.close_link()
class LpsRebootToBootloader: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._reboot_thread).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) def _reboot_thread(self): anchors = LoPoAnchor(self._cf) print('Sending reboot signal to all anchors 10 times in a row ...') for retry in range(10): for anchor_id in range(6): anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER) time.sleep(0.1) self._cf.close_link()
class MyCF: def __init__(self, uri): self.cf = Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) self.cf.open_link(uri) self.uri = uri self.is_connected = False def connected(self, uri): self.is_connected = True print("Connected to {}".format(uri)) def disconnected(self, uri): print("disconnected from {}".format(uri)) def close(self): self.cf.close_link() def start_motors(self): Thread(target=self.motor).start() def motor(self): thrust_mult = 1 thrust_step = 200 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 # Unlock startup thrust protection self.cf.commander.send_setpoint(0, 0, 0, 0) while thrust >= 15000: self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust) sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self.cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing sleep(0.1) self.close()
def main(): #show available links cflib.crtp.init_drivers() available = cflib.crtp.scan_interfaces() for i in available: print "Interface with URI [%s] found and name/comment [%s]" % (i[0], i[1]) crazyflie = Crazyflie() crazyflie.open_link("radio://0/10/250K") roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 35001 crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(5) crazyflie.close_link()
class Main: # Initial values, you can use these to set trim etc. roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 10001 _stop = 0; def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/1/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): print "Should be connected now...\n" # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() print "Beginning input loop:" while 1: try: command = raw_input("Set thrust (10001-60000) (0 will turn off the motors, e or q will quit):") if (command=="exitstring manipulation python") or (command=="e") or (command=="quit") or (command=="q"): # Exit command received # Set thrust to zero to make sure the motors turn off NOW self.thrust = 0 # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1; # Exit the main loop print "Exiting main loop in 1 second" time.sleep(1) self.crazyflie.close_link() # This errors out for some reason. Bad libusb? break
class Main: def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() print("Trying to open link") self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) time.sleep(10) t1 = threading.Thread(target=self.FlyThread) t1.start() print("Link is opened") def FlyThread(self): roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 10001 # minimum thrust required to power motors print("In FlyThread") #while(True): for x in range(0,100): result = self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust+x) time.sleep(0.1) self.crazyflie.close_link() def connectSetupFinished(self, linkURI): # Called when connection is done print("Connection is finished");
class Controller: def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.link_quality_updated.add_callback(self._quality_updated) self._cf.open_link(link_uri) self.quality = -1 self.failed = False def linkQuality(self): return self.quality def connectionFailed(self): return self.failed def closeLink(self): self._cf.close_link() def __enter__(self): return self def __exit__(self, exc_type, exc_value, traceback): self._cf.close_link() def __del__(self): self._cf.close_link() 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.""" self.failed = False def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" self.failed = True def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" self.failed = True def _quality_updated(self, quality): if quality < self.quality or self.quality == -1: self.quality = quality
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) while thrust >= 20000: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self._cf.close_link()
class formationControl(threading.Thread): """ A controller thread, taking references and mearsurements from the UAV computing a control input""" def __init__(self, link_uri): threading.Thread.__init__(self) #self.link_uri = link_uri self.daemon = True self.timePrint = 0.0 self.timeplt = time.time() # Initialize the crazyflie object and add some callbacks 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.rate = 5 # (Hz) Rate of each sending control input self.statefb_rate = 200 # (ms) Time between two consecutive state feedback # Logged states -, attitude(p, r, y), rotation matrix self.position = [0.0, 0.0, 0.0] # [m] in the global frame of reference self.velocity = [0.0, 0.0, 0.0] # [m/s] in the global frame of reference self.attitude = [0.0, 0.0, 0.0] # [rad] attitude with inverted roll (r) self.rotMat = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] self.Mtr = [0.0, 0.0, 0.0, 0.0, 0.0] # Multiranger # Limits self.thrust_limit = (30000, 50000) self.roll_limit = (-30.0, 30.0) # [Degree] self.pitch_limit = (-30.0, 30.0) # [Degree] self.yaw_limit = (-200.0, 200.0) # [Degree] # Control setting self.isEnabled = True self.printDis = True # System parameter self.pi = 3.14159265359 # Control keyboard self.keyboard_step = 0.1 self.keyboard_flag_a = 0.0 self.keyboard_flag_d = 0.0 self.keyboard_flag_w = 0.0 self.keyboard_flag_s = 0.0 self.keyboard_flag_j = 0.0 self.keyboard_flag_k = 0.0 def _run_controller(self): """ Main control loop # System parameters""" m = 0.034 # [kg] actual mass with some decks g = 9.799848 # [m/s^2] gravitational acceleration timeSampling = 1.0 / float(self.rate) # [s] sampling time number_Quad = 1 # [] number of crazyflie used delta = 2 # [] if quad knows reference -> 1, otherwise 0 # Control Parameters gv = 1.2 # for velocity gp = 0.5 # for position # Reference parameters v0 = [0.0, 0.0, 0.0] # Reference velocity r_amp = 0.5 r_omg = 0.2 r_k = 0.0 fp_ref = [0.0, 0.0, 0.0] # [m] the formation shape fp = [0.0, 0.0, 0.0] # [m] the formation shape fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape coef_init = 1.0 # initialize altitude rot_mat = [ [0.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 0.0, 0.0], # shape in 3D space [0.0, 0.0, 0.0] ] col_thrs = 0.4 safe_thrs = 0.7 col_muy = 0.3 col_lda = -0.0 Mtr_pre = self.Mtr Mtr_dot = [0.0, 0.0, 0.0, 0.0] # z compensation error_z = 0.0 error_z_pre = 0.0 z_ctrl_cp = 0.0 # Set the current reference to the current positional estimate time.sleep(5) # Unlock the controller # First, we need send a signal to enable sending attitude reference and thrust force self._cf.commander.send_setpoint(0, 0, 0, 0) # Hover at z = 0.3 durin 2 seconds x_0, y_0, z_0 = self.position yaw_0 = self.attitude[2] timeStart = time.time() while True: self._cf.commander.send_position_setpoint(x=x_0, y=y_0, z=0.4, yaw=yaw_0) time.sleep(0.2) if time.time() - timeStart > 2.0: break # Take a local coordinate x_0, y_0, z_0 = self.position yaw_d = self.attitude[2] print('Initial position reference: ({}, {}, {})'.format(x_0, y_0, z_0)) x_d, y_d, z_d = [x_0, y_0, z_0] # Begin while loop for sending the control signal timeBeginCtrl = time.time() while True: timeStart = time.time() # Change the reference velocity if time.time() > timeBeginCtrl + 0.5: #v0 = [0.0, -0.0, 0.0] #v0[0] = r_omg * r_amp * cos(r_omg * r_k * timeSampling) #v0[1] = - r_omg * r_amp * sin(r_omg * r_k * timeSampling) rot_mat = [ [1.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 1.0, 0.0], # shape in 3D space [0.0, 0.0, 1.0] ] coef_init = 0.0 r_k = r_k + 1.0 if time.time() > timeBeginCtrl + 60.0: self._cf.commander.send_setpoint(0, 0, 0, 39000) self._cf.close_link() print('Disconnect timeout') # Get the reference x_d = x_d + v0[0] * timeSampling y_d = y_d + v0[1] * timeSampling z_d = z_d + v0[2] * timeSampling if self.keyboard_flag_s == 1.0: x_d -= self.keyboard_step self.keyboard_flag_s = 0.0 elif self.keyboard_flag_w == 1.0: x_d += self.keyboard_step self.keyboard_flag_w = 0.0 elif self.keyboard_flag_d == 1.0: y_d -= self.keyboard_step self.keyboard_flag_d = 0.0 elif self.keyboard_flag_a == 1.0: y_d += self.keyboard_step self.keyboard_flag_a = 0.0 elif self.keyboard_flag_j == 1.0: z_d -= self.keyboard_step self.keyboard_flag_j = 0.0 elif self.keyboard_flag_k == 1.0: z_d += self.keyboard_step self.keyboard_flag_k = 0.0 # Storage data self.datalog_Pos.write( str(time.time()) + "," + str(self.position[0]) + "," + str(self.position[1]) + "," + str(self.position[2]) + "\n") # Send set point #self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, thrust_d) #self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, 43000) self._cf.commander.send_position_setpoint(x_d, y_d, z_d, yaw_d) #self.print_at_period(2.0, message) self.loop_sleep(timeStart) # End def _connected(self, link_uri): """ This callback is called from the Crazyflie API when a Crazyflie has been connected adn TOCs have been downloaded """ print('connected to: %s' % link_uri) # Open text file for recording data self.datalog_Pos = open("log_data/Cf2log_Pos", "w") #self.datalog_Vel = open("log_data/Cf2log_Vel","w") #self.datalog_Att = open("log_data/Cf2log_Att","w") self.datalog_Att = open("log_data/Cf2log_Mtr", "w") # Reset Kalman filter self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) print('Wait for Kalamn filter') # Feedback position estimated by KF logPos = LogConfig(name='Kalman Position', period_in_ms=self.statefb_rate) logPos.add_variable('kalman.stateX', 'float') logPos.add_variable('kalman.stateY', 'float') logPos.add_variable('kalman.stateZ', 'float') # Feedback velocity estimated by KF logVel = LogConfig(name='Kalman Velocity', period_in_ms=self.statefb_rate) logVel.add_variable('kalman.statePX', 'float') logVel.add_variable('kalman.statePY', 'float') logVel.add_variable('kalman.statePZ', 'float') # Feedback Quaternion attitude estimated by KF logAtt = LogConfig(name='Kalman Attitude', period_in_ms=self.statefb_rate) logAtt.add_variable('kalman.q0', 'float') logAtt.add_variable('kalman.q1', 'float') logAtt.add_variable('kalman.q2', 'float') logAtt.add_variable('kalman.q3', 'float') # Feedback Multi ranger logMultiRa = LogConfig(name='Range', period_in_ms=self.statefb_rate) logMultiRa.add_variable('range.front', 'uint16_t') logMultiRa.add_variable('range.back', 'uint16_t') logMultiRa.add_variable('range.left', 'uint16_t') logMultiRa.add_variable('range.right', 'uint16_t') logMultiRa.add_variable('range.up', 'uint16_t') # Invoke logged states self._cf.log.add_config(logPos) self._cf.log.add_config(logVel) self._cf.log.add_config(logAtt) self._cf.log.add_config(logMultiRa) # Start invoking logged states if logPos.valid and logVel.valid: # Position logPos.data_received_cb.add_callback(self.log_pos_callback) logPos.error_cb.add_callback(logging_error) logPos.start() # Velocity logVel.data_received_cb.add_callback(self.log_vel_callback) logVel.error_cb.add_callback(logging_error) logVel.start() # Quadternion attitude logAtt.data_received_cb.add_callback(self.log_att_callback) logAtt.error_cb.add_callback(logging_error) logAtt.start() # Multi-Ranger logMultiRa.data_received_cb.add_callback(self.log_mtr_callback) logMultiRa.error_cb.add_callback(logging_error) logMultiRa.start() else: print( "One or more of the variables in the configuration was not found" + "in log TOC. No logging will be possible") # Start in a thread threading.Thread(target=self._run_controller).start() def log_pos_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.position = [ data['kalman.stateX'], data['kalman.stateY'], data['kalman.stateZ'] ] #self.datalog_Pos.write(str([time.time(), self.position]) + "\n") #print('Position x, y, z, time', self.position, time.time()) #print(self.rate) def log_att_callback(self, timestamp, data, Logconf): """ Callback for the logging the quadternion attitude of the UAV which is converted to roll, pitch, yaw in radians""" q = [ data['kalman.q0'], data['kalman.q1'], data['kalman.q2'], data['kalman.q3'] ] #Convert the quadternion to pitch roll and yaw yaw = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) pitch = asin(-2 * (q[1] * q[3] - q[0] * q[2])) roll = atan2(2 * (q[2] * q[3] + q[0] * q[1]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]) self.attitude = [roll, pitch, yaw] # Convert the quaternion to a rotation matrix R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] R[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3] R[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3] R[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2] R[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3] R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3] R[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1] R[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2] R[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1] R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3] self.rotMat = R #self.datalog_Att.write(str([time.time(), self.attitude]) + "\n") #print('Attitude r, p, y', self.attitude) def log_vel_callback(self, timestamp, data, Logconf): """ Callback for logging the velocity of the UAV defined w.r.t the body frame this subsequently rotated to the global frame""" PX, PY, PZ = [ data['kalman.statePX'], data['kalman.statePY'], data['kalman.statePZ'] ] R = self.rotMat self.velocity[0] = R[0][0] * PX + R[0][1] * PY + R[0][2] * PZ self.velocity[1] = R[1][0] * PX + R[1][1] * PY + R[1][2] * PZ self.velocity[2] = R[2][0] * PX + R[2][1] * PY + R[2][2] * PZ #self.datalog_Vel.write(str([time.time(), self.velocity]) + "\n") #print('Velocity rx, ry, rz', self.velocity) def log_mtr_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.Mtr = [ data['range.front'] * 0.001, data['range.back'] * 0.001, data['range.left'] * 0.001, data['range.right'] * 0.001, data['range.up'] * 0.001, ] #print('Time, Multirange: front, back, left, right, up', time.time(), self.Mtr) def _connection_failed(self, link_uri, msg): """ Callback when connection initial connection fails there is no Crazyflie at the specified address """ print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """ Callback when disconected after a connection has been made """ print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """ Callback when the Crazyflie is disconnected (called in all cases) """ if self.printDis: print('Disconnected from %s' % link_uri) self.printDis = False def loop_sleep(self, timeStart): """ Sleeps the control loop to make it run at a specified rate """ deltaTime = 1.0 / float(self.rate) - (time.time() - timeStart) if deltaTime > 0: time.sleep(deltaTime) def _close_connection(self, message): """ This is able to close link connecting Crazyflie from keyboard """ if message == "q": self.isEnabled = False # end def set_reference(self, message): """ Enables an incremental change in the reference and defines the keyboard mapping (change to your preference, but if so, also make sure to change the valid_keys attribute in the interface thread)""" verbose = True if message == "s": self.keyboard_flag_s = 1.0 if message == "w": self.keyboard_flag_w = 1.0 if message == "d": self.keyboard_flag_d = 1.0 if message == "a": self.keyboard_flag_a = 1.0 if message == "j": self.keyboard_flag_j = 1.0 if message == "k": self.keyboard_flag_k = 1.0
class Main: def __init__(self): self.thrust = 25000 self.pitch = -4 self.roll = 0 self.yaw = 0 self.stopping = False self.jump = 0 self.backward = 0 self.forward = 0 Thread(target=self.gui).start() self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. #self.crazyflie.open_link("radio://0/7/250K") self.crazyflie.open_link("radio://0/10/250K") #self.crazyflie.open_link("radio://0/6/1M") self.crazyflie.connectSetupFinished.add_callback( self.connectSetupFinished) def connectSetupFinished(self, linkURI): # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self.pulse_command).start() def gui(self): print "bingo" while self.stopping == False: #nb = _GetchUnix() nb = sys.stdin.read(1) if nb == 'x': self.stopping = True if nb == 'r': self.thrust = self.thrust + 1000 if nb == 'f': self.thrust = self.thrust - 1000 if nb == 'y': self.backward = 0 self.forward = 3 if nb == 'h': self.backward = 3 self.forward = 0 if nb == '3': self.thrust = 35000 if nb == '4': self.thrust = 40000 if nb == 'e': self.yaw = self.yaw + 1 if nb == 'q': self.yaw = self.yaw - 1 if nb == 'd': self.roll = self.roll + 2 if nb == 'a': self.roll = self.roll - 2 if nb == 'w': self.pitch = self.pitch - 2 if nb == 's': self.pitch = self.pitch + 2 if nb == 'z': self.jump = 2 sys.stdout.write("thrust=") print self.thrust sys.stdout.write("yaw=") print self.yaw sys.stdout.write("pitch=") print self.pitch sys.stdout.write("roll=") print self.roll def pulse_command(self): while self.stopping == False: lthrust = self.thrust lpitch = self.pitch if self.jump > 0: lthrust = self.thrust + 23000 self.jump = self.jump - 1 if self.forward > 0: lpitch = self.pitch + 4 self.forward = self.forward - 1 if self.backward > 0: lpitch = self.pitch - 4 self.backward = self.backward - 1 self.crazyflie.commander.send_setpoint(self.roll, lpitch, self.yaw, lthrust) time.sleep(0.15) self.crazyflie.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self.crazyflie.close_link()
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 MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) ###################################################### # By lxrocks # 'Skinny Progress Bar' tweak for Yosemite # Tweak progress bar - artistic I am not - so pick your own colors !!! # Only apply to Yosemite ###################################################### import platform if platform.system() == 'Darwin': (Version, junk, machine) = platform.mac_ver() logger.info("This is a MAC - checking if we can apply Progress " "Bar Stylesheet for Yosemite Skinny Bars ") yosemite = (10, 10, 0) tVersion = tuple(map(int, (Version.split(".")))) if tVersion >= yosemite: logger.info("Found Yosemite - applying stylesheet") tcss = """ QProgressBar { border: 1px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: """ + COLOR_BLUE + """; } """ self.setStyleSheet(tcss) else: logger.info("Pre-Yosemite - skinny bar stylesheet not applied") ###################################################### self.cf = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.crtp.init_drivers(enable_debug_driver=Config() .get("enable_debug_driver")) zmq_params = ZMQParamAccess(self.cf) zmq_params.start() zmq_leds = ZMQLEDDriver(self.cf) zmq_leds.start() self.scanner = ScannerThread() self.scanner.interfaceFoundSignal.connect(self.foundInterfaces) self.scanner.start() # Create and start the Input Reader self._statusbar_label = QLabel("No input-device found, insert one to" " fly.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" # self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) # TODO: Need to reload configs # ConfigManager().conf_needs_reload.add_callback(self._reload_configs) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect( self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Hide the 'File' menu on OS X, since its only item, 'Exit', gets # merged into the application menu. if sys.platform == 'darwin': self.menuFile.menuAction().setVisible(False) # Connect UI signals self.logConfigAction.triggered.connect(self._show_connect_dialog) self.interfaceCombo.currentIndexChanged['QString'].connect( self.interfaceChanged) self.connectButton.clicked.connect(self._connect) self.scanButton.clicked.connect(self._scan) self.menuItemConnect.triggered.connect(self._connect) self.menuItemConfInputDevice.triggered.connect( self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_battery) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self._disconnected) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(self._connection_initiated) self._log_error_signal.connect(self._logging_error) self.batteryBar.setTextVisible(False) self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) self.linkQualityBar.setTextVisible(False) self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) self._selected_interface = None self._initial_scan = True self._scan() # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t is not None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # References to all the device sub-menus in the "Input device" menu self._all_role_menus = () # Used to filter what new devices to add default mapping to self._available_devices = () # Keep track of mux nodes so we can enable according to how many # devices we have self._all_mux_nodes = () # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m.name, self._menu_inputdevice, checkable=True, enabled=False) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_inputdevice.addAction(node) self._all_mux_nodes += (node,) mux_subnodes = () for name in m.supported_roles(): sub_node = QMenu(" {}".format(name), self._menu_inputdevice, enabled=False) self._menu_inputdevice.addMenu(sub_node) mux_subnodes += (sub_node,) self._all_role_menus += ({"muxmenu": node, "rolemenu": sub_node},) node.setData((m, mux_subnodes)) self._mapping_support = True def interfaceChanged(self, interface): if interface == INTERFACE_PROMPT_TEXT: self._selected_interface = None else: self._selected_interface = interface self._update_ui_state() def foundInterfaces(self, interfaces): selected_interface = self._selected_interface self.interfaceCombo.clear() self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT) formatted_interfaces = [] for i in interfaces: if len(i[1]) > 0: interface = "%s - %s" % (i[0], i[1]) else: interface = i[0] formatted_interfaces.append(interface) self.interfaceCombo.addItems(formatted_interfaces) if self._initial_scan: self._initial_scan = False try: selected_interface = Config().get("link_uri") except KeyError: pass newIndex = 0 if selected_interface is not None: try: newIndex = formatted_interfaces.index(selected_interface) + 1 except ValueError: pass self.interfaceCombo.setCurrentIndex(newIndex) self.uiState = UIState.DISCONNECTED self._update_ui_state() def _update_ui_state(self): if self.uiState == UIState.DISCONNECTED: self.setWindowTitle("Not connected") canConnect = self._selected_interface is not None self.menuItemConnect.setText("Connect to Crazyflie") self.menuItemConnect.setEnabled(canConnect) self.connectButton.setText("Connect") self.connectButton.setToolTip( "Connect to the Crazyflie on the selected interface") self.connectButton.setEnabled(canConnect) self.scanButton.setText("Scan") self.scanButton.setEnabled(True) self.address.setEnabled(True) self.batteryBar.setValue(3000) self._menu_cf2_config.setEnabled(False) self._menu_cf1_config.setEnabled(True) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) self.interfaceCombo.setEnabled(True) elif self.uiState == UIState.CONNECTED: s = "Connected on %s" % self._selected_interface self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Disconnect") self.connectButton.setToolTip("Disconnect from the Crazyflie") self.scanButton.setEnabled(False) self.logConfigAction.setEnabled(True) # Find out if there's an I2C EEPROM, otherwise don't show the # dialog. if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0: self._menu_cf2_config.setEnabled(True) self._menu_cf1_config.setEnabled(False) elif self.uiState == UIState.CONNECTING: s = "Connecting to {} ...".format(self._selected_interface) self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Cancel") self.connectButton.setToolTip("Cancel connecting to the Crazyflie") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) elif self.uiState == UIState.SCANNING: self.setWindowTitle("Scanning ...") self.connectButton.setText("Connect") self.menuItemConnect.setEnabled(False) self.connectButton.setText("Connect") self.connectButton.setEnabled(False) self.scanButton.setText("Scanning...") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() # for c in self._menu_mappings.actions(): # c.setEnabled(False) # devs = self.joystickReader.available_devices() # if (len(devs) > 0): # self.device_discovery(devs) def _show_input_device_config_dialog(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked Config().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: {}".format(checked)) def _show_connect_dialog(self): self.logConfigDialogue.show() def _update_battery(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) color = COLOR_BLUE # TODO firmware reports fully-charged state as 'Battery', # rather than 'Charged' if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]: color = COLOR_GREEN elif data["pm.state"] == BatteryStates.LOW_POWER: color = COLOR_RED self.batteryBar.setStyleSheet(progressbar_stylesheet(color)) def _connected(self): self.uiState = UIState.CONNECTED self._update_ui_state() Config().set("link_uri", str(self._selected_interface)) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") lg.add_variable("pm.state", "int8_t") try: self.cf.log.add_config(lg) lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() except KeyError as e: logger.warning(str(e)) mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0] mem.write_data(self._led_write_done) # self._led_write_test = 0 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.write_data(self._led_write_done) def _disconnected(self): self.uiState = UIState.DISCONNECTED self._update_ui_state() def _connection_initiated(self): self.uiState = UIState.CONNECTING self._update_ui_state() def _led_write_done(self, mem, addr): logger.info("LED write done callback") # self._led_write_test += 1 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.write_data(self._led_write_done) def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [{}]: {}".format(log_conf.name, msg)) def _connection_lost(self, linkURI, msg): if not self._auto_reconnect_enabled: if self.isActiveWindow(): warningCaption = "Communication failure" error = "Connection lost to {}: {}".format(linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def _connection_failed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on {}: {}".format(linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def _connect(self): if self.uiState == UIState.CONNECTED: self.cf.close_link() elif self.uiState == UIState.CONNECTING: self.cf.close_link() self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self.cf.open_link(self._selected_interface) def _scan(self): self.uiState = UIState.SCANNING self._update_ui_state() self.scanner.scanSignal.emit(self.address.value()) def _display_input_device_error(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _mux_selected(self, checked): """Called when a new mux is selected. The menu item contains a reference to the raw mux object as well as to the associated device sub-nodes""" if not checked: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(False) else: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(True) self.joystickReader.set_mux(mux=mux) # Go though the tree and select devices/mapping that was # selected before it was disabled. for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): dev_node.toggled.emit(True) self._update_input_device_footer() def _get_dev_status(self, device): msg = "{}".format(device.name) if device.supports_mapping: map_name = "N/A" if device.input_map: map_name = device.input_map_name msg += " ({})".format(map_name) return msg def _update_input_device_footer(self): """Update the footer in the bottom of the UI with status for the input device and its mapping""" msg = "" if len(self.joystickReader.available_devices()) > 0: mux = self.joystickReader._selected_mux msg = "Using {} mux with ".format(mux.name) for key in list(mux._devs.keys())[:-1]: if mux._devs[key]: msg += "{}, ".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A, " # Last item key = list(mux._devs.keys())[-1] if mux._devs[key]: msg += "{}".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A" else: msg = "No input device found" self._statusbar_label.setText(msg) def _inputdevice_selected(self, checked): """Called when a new input device has been selected from the menu. The data in the menu object is the associated map menu (directly under the item in the menu) and the raw device""" (map_menu, device, mux_menu) = self.sender().data() if not checked: if map_menu: map_menu.setEnabled(False) # Do not close the device, since we don't know exactly # how many devices the mux can have open. When selecting a # new mux the old one will take care of this. else: if map_menu: map_menu.setEnabled(True) (mux, sub_nodes) = mux_menu.data() for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): if device.id == dev_node.data()[1].id \ and dev_node is not self.sender(): dev_node.setChecked(False) role_in_mux = str(self.sender().parent().title()).strip() logger.info("Role of {} is {}".format(device.name, role_in_mux)) Config().set("input_device", str(device.name)) self._mapping_support = self.joystickReader.start_input( device.name, role_in_mux) self._update_input_device_footer() def _inputconfig_selected(self, checked): """Called when a new configuration has been selected from the menu. The data in the menu object is a referance to the device QAction in parent menu. This contains a referance to the raw device.""" if not checked: return selected_mapping = str(self.sender().text()) device = self.sender().data().data()[1] self.joystickReader.set_input_map(device.name, selected_mapping) self._update_input_device_footer() def device_discovery(self, devs): """Called when new devices have been added""" for menu in self._all_role_menus: role_menu = menu["rolemenu"] mux_menu = menu["muxmenu"] dev_group = QActionGroup(role_menu, exclusive=True) for d in devs: dev_node = QAction(d.name, role_menu, checkable=True, enabled=True) role_menu.addAction(dev_node) dev_group.addAction(dev_node) dev_node.toggled.connect(self._inputdevice_selected) map_node = None if d.supports_mapping: map_node = QMenu(" Input map", role_menu, enabled=False) map_group = QActionGroup(role_menu, exclusive=True) # Connect device node to map node for easy # enabling/disabling when selection changes and device # to easily enable it dev_node.setData((map_node, d)) for c in ConfigManager().get_list_of_configs(): node = QAction(c, map_node, checkable=True, enabled=True) node.toggled.connect(self._inputconfig_selected) map_node.addAction(node) # Connect all the map nodes back to the device # action node where we can access the raw device node.setData(dev_node) map_group.addAction(node) # If this device hasn't been found before, then # select the default mapping for it. if d not in self._available_devices: last_map = Config().get("device_config_mapping") if d.name in last_map and last_map[d.name] == c: node.setChecked(True) role_menu.addMenu(map_node) dev_node.setData((map_node, d, mux_menu)) # Update the list of what devices we found # to avoid selecting default mapping for all devices when # a new one is inserted self._available_devices = () for d in devs: self._available_devices += (d,) # Only enable MUX nodes if we have enough devies to cover # the roles for mux_node in self._all_mux_nodes: (mux, sub_nodes) = mux_node.data() if len(mux.supported_roles()) <= len(self._available_devices): mux_node.setEnabled(True) # TODO: Currently only supports selecting default mux if self._all_mux_nodes[0].isEnabled(): self._all_mux_nodes[0].setChecked(True) # If the previous length of the available devies was 0, then select # the default on. If that's not available then select the first # on in the list. # TODO: This will only work for the "Normal" mux so this will be # selected by default if Config().get("input_device") in [d.name for d in devs]: for dev_menu in self._all_role_menus[0]["rolemenu"].actions(): if dev_menu.text() == Config().get("input_device"): dev_menu.setChecked(True) else: # Select the first device in the first mux (will always be "Normal" # mux) self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True) logger.info("Select first device") self._update_input_device_footer() def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(cfclient.config_path))) def closeAppRequest(self): self.close() sys.exit(0)
class OWExample: """ Simple example listing the 1-wire memories found and lists its contents. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # 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 self._mems_to_update = 0 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) mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) self._mems_to_update = len(mems) print('Found {} 1-wire memories'.format(len(mems))) for m in mems: print('Updating id={}'.format(m.id)) m.update(self._data_updated) def _data_updated(self, mem): print('Updated id={}'.format(mem.id)) print('\tAddr : {}'.format(mem.addr)) print('\tType : {}'.format(mem.type)) print('\tSize : {}'.format(mem.size)) print('\tValid : {}'.format(mem.valid)) print('\tName : {}'.format(mem.name)) print('\tVID : 0x{:02X}'.format(mem.vid)) print('\tPID : 0x{:02X}'.format(mem.pid)) print('\tPins : 0x{:02X}'.format(mem.pins)) print('\tElements : ') for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) self._mems_to_update -= 1 if self._mems_to_update == 0: self._cf.close_link() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) 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 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)) 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
class UpDown: """docstring for UpDown""" def __init__(self, link_uri): self._cf = Crazyflie() 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._logger = LogConfig(name='Logger', period_in_ms=10) self._logger.add_variable('acc.x', 'float') self._logger.add_variable('acc.y', 'float') self._logger.add_variable('acc.z', 'float') self._logger.add_variable('acc.zw', 'float') self._acc_x = 0 self._acc_y = 0 self._acc_z = 0 self._acc_zw = 0.0001 self.log = [] self.pidLog = [] self.acc_pid_x = None self.acc_pid_y = None self.acc_pid_z = None self.vel_pid_x = None self.vel_pid_y = None self.vel_pid_z = None print("Connecting to %s" % link_uri) self._cf.open_link(link_uri) self.is_connected = True self.exit = False self.init = False Thread(target=self._exit_task).start() Thread(target=self._run_task).start() 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.""" self.initTime = time.clock() # self.log_file = open('log.txt', 'w') # self.pid_log = open('pid.txt', 'w') # self.log_file.write('[\n') # self.pid_log.write('[\n') try: self._cf.log.add_config(self._logger) # This callback will receive the data self._logger.data_received_cb.add_callback(self._acc_log_data) # This callback will be called on errors self._logger.error_cb.add_callback(self._acc_log_error) # Start the logging self._logger.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Logger log config, bad configuration.') def _acc_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _acc_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" self._acc_x = float(data['acc.x']) self._acc_y = float(data['acc.y']) self._acc_z = float(data['acc.z']) self._acc_zw = data['acc.zw'] # self.log_file.write(json.dumps(data, sort_keys=True) + ",\n") self.log.append(data) self.init = True 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.is_connected = False # self.log_file.write(']') # self.pid_log.write(']') # self.log_file.close() # self.pid_log.close() with open('log.txt', 'w') as logFile: json.dump(self.log, logFile) with open('pid.txt', 'w') as pidFile: json.dump(self.pidLog, pidFile) 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.log_file.write(']') # self.pid_log.write(']') # self.log_file.close() # self.pid_log.close() with open('log.txt', 'w') as logFile: json.dump(self.log, logFile) with open('pid.txt', 'w') as pidFile: json.dump(self.pidLog, pidFile) 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 with open('log.txt', 'w') as logFile: json.dump(self.log, logFile) with open('pid.txt', 'w') as pidFile: json.dump(self.pidLog, pidFile) def _exit_task(self): while not self.exit: inp = int(input('Want to exit? [NO:0/YES:1]\n')) if inp == 1: self.exit = 1 def _run_task(self): print("Running thread") self._cf.commander.send_setpoint(0, 0, 0, 0) while not self.exit: if not self.init: self.acc_pid_x = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1) self.acc_pid_y = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1) self.acc_pid_z = pid.PID(0, 0, 100, 0.0005, 0.1, 0.4, 2) # Thread(target=self._log_pid).start() continue inp_acc_x = self._acc_x self.out_acc_x, self.err_acc_x = self.acc_pid_x.compute(inp_acc_x) self.setPitch = -self.out_acc_x*10 inp_acc_y = self._acc_y self.out_acc_y, self.err_acc_y = self.acc_pid_y.compute(inp_acc_y) self.setRoll = self.out_acc_y*10 inp_acc_z = self._acc_zw self.out_acc_z, self.err_acc_z = self.acc_pid_z.compute(self._acc_zw) self.setThrust = int(60000*self.out_acc_z/2) self._thrust = self.setThrust # data = {"IN_X": inp_acc_x, "OUT_X": out_acc_x, # "IN_Y": inp_acc_y, "OUT_Y": out_acc_y, # "IN_Z": self._acc_zw, "OUT_Z": out_acc_z, # "OUT_Roll": setRoll, "OUT_Pitch": setPitch, # "OUT_Thrust": setThrust} # self.pidLog.append(data) # self.pid_log.write(json.dumps(data, sort_keys=True) + ",\n") self._cf.commander.send_setpoint( self.setRoll, self.setPitch, 0, self._thrust) time.sleep(0.01) # self.log_file.write(']') # self.pid_log.write(']') # self.log_file.close() # self.pid_log.close() self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(1) self._cf.close_link() def _log_pid(self): data = {"IN_X": self._acc_x, "OUT_X": self.out_acc_x, "IN_Y": self._acc_y, "OUT_Y": self.out_acc_y, "IN_Z": self._acc_zw, "OUT_Z": self.out_acc_z, "OUT_Roll": self.setRoll, "OUT_Pitch": self.setPitch, "OUT_Thrust": self.setThrust} self.pidLog.append(data)
class MyFirstExample: """MyExamples""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) # Variable used to keep main loop occupied until disconnect #self.is_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.""" #setup log config for sensor #self._log_sensor = LogConfig("Logs", 200) #self._log_sensor.add_variable("adc.vProx") #self._log_sensor.add_variable("baro.aslLong") #self._cf.log.add_config(self._log_sensor) #if self._log_sensor.valid: # This callback will receive the data # self._log_sensor.data_received_cb.add_callback(self._log_data) # This callback will be called on errors # self._log_sensor.error_cb.add_callback(self._log_error) # Start the logging # self._log_sensor.start() #else: # print("Could not add logconfig since some variables are not in TOC") # Start a timer to disconnect in 5s print("Connected, starting timer") t = Timer(7, self._landing) t.start() # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() #self._ramp_motors() print("sleeping 20") time.sleep(20) def _log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" #global ref_baro #ref_baro = data["baro.aslLong"] #print(ref_baro) #print "[%d][%s]: %s" % (timestamp, logconf.name, data) def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) 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 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) 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 def _landing(self): thrust_mult = -1 thrust_step = 200 thrust = 20000 global landing landing = True print "Landing ON" self._cf.commander.send_setpoint(0, 0, 0, 0) self._cf.param.set_value("flightmode.althold","False") while thrust >= 15000: self._cf.commander.send_setpoint(0, 0, 0, thrust) thrust += thrust_step * thrust_mult time.sleep(0.1) print ("Landing thurst:",thrust) time.sleep(0.1) self._cf.close_link() def _ramp_motors(self): #thrust_mult = 1 #thrust_step = 100 thrust = 15000 #pitch = 0 #roll = 0 #yawrate = 0 global landing landing = False # print(ref_baro) #while thrust >= 20000: # self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) # time.sleep(0.1) # if thrust >= 25000: # thrust_mult = -1 # thrust += thrust_step * thrust_mult #self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing # H O V E R # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) print ("Starting motors thurst:",thrust) self._cf.commander.send_setpoint(0,0,0,thrust) time.sleep(0.1) print "Hover ON" #self._cf.param.set_value("flightmode.althold","True") while landing == False: thrust = 32767 #self._cf.commander.send_setpoint(0,0,0,32767) print ("Hovering thurst:",thrust) time.sleep(0.1)
class Main: # Initial values, you can use these to set trim etc. roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 10001 _stop = 0; def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): print "Should be connected now...\n" # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() print "Beginning input loop:" while 1: try: command = raw_input("Set thrust (10001-60000) (0 will turn off the motors, e or q will quit):") if (command=="exit") or (command=="e") or (command=="quit") or (command=="q"): # Exit command received # Set thrust to zero to make sure the motors turn off NOW self.thrust = 0 # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1; # Exit the main loop print "Exiting main loop in 1 second" time.sleep(1) self.crazyflie.close_link() # This errors out for some reason. Bad libusb? break elif (self.is_number(command)): # Good thrust value self.thrust = int(command) if self.thrust == 0: self.thrust = 0 elif self.thrust <= 10000: self.thrust = 10001 elif self.thrust > 60000: self.thrust = 60000 print "Setting thrust to %i" % (self.thrust) else: print "Bad thrust value. Enter a number or e to exit" except: print "Exception thrown! Trying to continue!", sys.exc_info()[0] def is_number(self, s): try: int(s) return True except ValueError: return False def pulse_command(self): while 1: self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust) time.sleep(0.1) # Exit if the parent told us to if self._stop==1: print "Exiting keep alive thread" return
class MyFirstExample: """MyExamples""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) # Variable used to keep main loop occupied until disconnect #self.is_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.""" #setup log config for sensor self._log_sensor = LogConfig("Proximity sensor", 200) self._log_sensor.add_variable("adc.vProx") self._log_sensor.add_variable("baro.aslLong") self._cf.log.add_config(self._log_sensor) if self._log_sensor.valid: # This callback will receive the data self._log_sensor.data_received_cb.add_callback(self._log_data) # This callback will be called on errors self._log_sensor.error_cb.add_callback(self._log_error) # Start the logging self._log_sensor.start() else: print("Could not add logconfig since some variables are not in TOC") # Start a timer to disconnect in 5s t = Timer(10, self._cf.close_link) t.start() # Start a separate thread to do the motor test. # Do not hijack the calling thread! #Thread(target=self._ramp_motors).start() def _log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global myprox global ref_baro myprox = data["adc.vProx"] ref_baro = data["baro.aslLong"] #long = float(data["adc.vProx"]) #if myprox >= 2: # print(myprox, ref_baro) #else: # print("Sensor range low", test) Thread(target=self._ramp_motors).start() #print "[%d][%s]: %s" % (timestamp, logconf.name, data) def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) 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 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) 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 def _ramp_motors(self): thrust_mult = 1 thrust_step = 100 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 print(myprox, ref_baro) while thrust >= 20000: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing # H O V E R #cf.commander.send_setpoint(0,0,0,initial_thrust); #time.sleep(0.5); #cf.param.set_value("flightmode.althold","True"); # while some_condition: # cf.commander.send_setpoint(0,0,0,32767); # time.sleep(0.01); time.sleep(0.1) self._cf.close_link()
class Drone: """Represents a CrazyFlie drone.""" def __init__(self, drone_id: str, arena: Arena, radio_id: int = 0, channel: int = 80, address: str = "E7E7E7E7E7", data_rate: str = "2M"): """ Initializes the drone with the given uri.""" # Initialize public variables self.id: str = drone_id self.var_x: float = 0 self.var_y: float = 0 self.var_z: float = 0 self.pos_x: float = 0 self.pos_y: float = 0 self.pos_z: float = 0 self.pitch: float = 0 self.roll: float = 0 self.yaw: float = 0 self.battery_voltage: float = 0 self.is_connected: bool = False self.status: DroneState = DroneState.OFFLINE self.link_uri: str = "radio://" + str(radio_id) + "/" + str( channel) + "/" + data_rate + "/" + address # Initialize limits self._max_velocity: float = 1.0 self._min_duration: float = 1.0 self._max_yaw_rotations: float = 1.0 self._arena = arena # Event to asynchronously wait for the connection self._connect_event = threading.Event() # Initialize the crazyflie self._cf = Crazyflie(rw_cache='./cache') # Initialize the callbacks 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) # Initialize events self.drone_lost = Caller() # Define the log configuration self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500) self._log_config_1.add_variable('kalman.varPX', 'float') self._log_config_1.add_variable('kalman.varPY', 'float') self._log_config_1.add_variable('kalman.varPZ', 'float') self._log_config_1.add_variable('pm.vbat', 'float') self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=100) self._log_config_2.add_variable('kalman.stateX', 'float') self._log_config_2.add_variable('kalman.stateY', 'float') self._log_config_2.add_variable('kalman.stateZ', 'float') self._log_config_3 = LogConfig(name='DroneLog_3', period_in_ms=500) self._log_config_3.add_variable('stabilizer.pitch', 'float') self._log_config_3.add_variable('stabilizer.roll', 'float') self._log_config_3.add_variable('stabilizer.yaw', 'float') def connect(self, synchronous: bool = False): """Connects to the Crazyflie.""" self._connect_crazyflie() if synchronous: self._connect_event.wait() def disconnect(self): """Disconnects from the Crazyflie and stops all logging.""" self._disconnect_crazyflie() def enable_high_level_commander(self): """Enables the drones high level commander.""" self._cf.param.set_value('commander.enHighLevel', '1') time.sleep(0.1) def disable_motion_tracking(self): """Disables to motion control (x/y) from the flow-deck.""" self._cf.param.set_value('motion.disable', '1') time.sleep(0.1) def get_status(self) -> str: """Gets various information of the drone.""" return { "id": self.id, "var_x": self.var_x, "var_y": self.var_y, "var_z": self.var_z, "x": self._arena.transform_x_inverse(self.pos_x), "y": self._arena.transform_y_inverse(self.pos_y), "z": self.pos_z, "pitch": self.pitch, "roll": self.roll, "yaw": self.yaw, "status": self.status.name, "battery_voltage": self.battery_voltage, "battery_percentage:": (self.battery_voltage - 3.4) / (4.18 - 3.4) * 100 } def reset_estimator(self) -> bool: """Resets the position estimates.""" self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2.0) # TODO: wait_for_position_estimator(cf) return True def takeoff(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) self.reset_estimator() duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.takeoff(absolute_height, duration) self.status = DroneState.STARTING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def land(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.land(absolute_height, duration) self.status = DroneState.LANDING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def go_to(self, x: float, y: float, z: float, yaw: float, velocity: float, relative: bool = False, synchronous: bool = False) -> float: x = self._sanitize_x(x, relative) y = self._sanitize_y(y, relative) z = self._sanitize_z(z, relative) yaw = self._sanitize_yaw(yaw) distance = self._calculate_distance(x, y, z, relative) duration = self._convert_velocity_to_time(distance, velocity) self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative) self.status = DroneState.NAVIGATING if synchronous: time.sleep(duration) return { "duration": duration, "target_x": self._arena.transform_x_inverse(x), "target_y": self._arena.transform_y_inverse(y), "target_z": z, "target_yaw": yaw, "relative": relative } def stop(self): self._cf.high_level_commander.stop() self.status = DroneState.IDLE def _connect_crazyflie(self): print('Connecting to %s' % self.link_uri) self._cf.open_link(self.link_uri) def _disconnect_crazyflie(self): print('Disconnecting from %s' % self.link_uri) # Stop the loggers self._log_config_1.stop() self._log_config_2.stop() self._log_config_3.stop() # Shutdown the rotors self._shutdown() # Disconnect self._cf.close_link() def _connected(self, link_uri): """This callback is called when the Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) # Setup parameters self.disable_motion_tracking() # Add the logger self._cf.log.add_config(self._log_config_1) self._cf.log.add_config(self._log_config_2) self._cf.log.add_config(self._log_config_3) # This callback will receive the data self._log_config_1.data_received_cb.add_callback( self._log_config_1_data) self._log_config_2.data_received_cb.add_callback( self._log_config_2_data) self._log_config_3.data_received_cb.add_callback( self._log_config_3_data) # This callback will be called on errors self._log_config_1.error_cb.add_callback(self._log_config_error) self._log_config_2.error_cb.add_callback(self._log_config_error) self._log_config_3.error_cb.add_callback(self._log_config_error) # Start the logging self._log_config_1.start() self._log_config_2.start() self._log_config_3.start() # Set the connected event self._connect_event.set() self.is_connected = True self.status = DroneState.IDLE def _connection_failed(self, link_uri, msg): """Callback when the initial connection fails.""" print('Connection to %s failed: %s' % (link_uri, msg)) # Set the connected event self._connect_event.set() def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected.""" print('Disconnected from %s' % link_uri) self.is_connected = False self.status = DroneState.OFFLINE def _connection_lost(self, link_uri, msg): """Callback when the connection is lost after a connection has been made.""" print('Connection to %s lost: %s' % (link_uri, msg)) self.drone_lost.call(self) self._connect_event.set() self.is_connected = False self.status = DroneState.OFFLINE def _log_config_error(self, logconf, msg): """Callback from the log API when an error occurs.""" print('Error when logging %s: %s' % (logconf.name, msg)) def _log_config_1_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.var_x = data['kalman.varPX'] self.var_y = data['kalman.varPY'] self.var_z = data['kalman.varPZ'] self.battery_voltage = data['pm.vbat'] def _log_config_2_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.pos_x = data['kalman.stateX'] self.pos_y = data['kalman.stateY'] self.pos_z = data['kalman.stateZ'] def _log_config_3_data(self, timestamp, data, logconf): self.pitch = data['stabilizer.pitch'] self.roll = data['stabilizer.roll'] self.yaw = data['stabilizer.yaw'] def _unlock(self): # Unlock startup thrust protection (only needed for low lewel commands) self._cf.commander.send_setpoint(0, 0, 0, 0) def _shutdown(self): self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime): """Keeps the drone at the given setpoint for the given amount of time.""" while keeptime > 0: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) keeptime -= 0.1 time.sleep(0.1) def _convert_velocity_to_time(self, distance: float, velocity: float) -> float: """Converts a distance and a velocity to a time.""" duration = distance / self._sanitize_velocity(velocity) return self._sanitize_duration(duration) def _calculate_distance(self, x: float, y: float, z: float, relative: bool = False) -> float: """Calculates the distance from the drone or the zero position (relative) to a given point in space.""" start_x = 0 if relative else self.pos_x start_y = 0 if relative else self.pos_y start_z = 0 if relative else self.pos_z return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2) def _sanitize_velocity(self, velocity: float) -> float: return min(velocity, self._max_velocity) def _sanitize_duration(self, duration: float) -> float: return max(duration, self._min_duration) def _sanitize_yaw(self, yaw: float) -> float: return yaw % (2 * self._max_yaw_rotations * math.pi) def _sanitize_x(self, x: float, relative: bool) -> float: target_x = (self.pos_x + x) if relative else x sanitized_x = self._sanitize_number(target_x, self._arena.min_x, self._arena.max_x) return self._arena.transform_x(sanitized_x) def _sanitize_y(self, y: float, relative: bool) -> float: target_y = (self.pos_y + y) if relative else y sanitized_y = self._sanitize_number(target_y, self._arena.min_y, self._arena.max_y) return self._arena.transform_y(sanitized_y) def _sanitize_z(self, z: float, relative: bool) -> float: return self._sanitize_number(z, self._arena.min_z, self._arena.max_z) def _sanitize_number(self, value: float, min_value: float, max_value: float) -> float: return min(max(value, min_value), max_value)
class Drone: thruster = 0 yaw = 0 pitch = 0 roll = 0 connected = False thrust_flag = False def __init__(self, link_uri): self._cf = Crazyflie() 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) 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._ramp_motors).start() print "connected to %s" % link_uri print("before Logconfig") Drone.connected = True 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) 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) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _motor_control(self): print "Motor control started!!" base_thrust_value = 35000 thrust_step = 8000 thrust = base_thrust_value pitch_rate = 10 roll_rate = 10 yaw_rate = 0 # win = pg.GraphicsWindow() # win.setWindowTitle('pyqtgraph example: Scrolling Plots') # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=100) self._lg_stab.add_variable("stabilizer.thrust", "float") self._lg_stab.add_variable("stabilizer.roll", "float") self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we # would like to log are in the TOC. try: self._cf.log.add_config(self._lg_stab) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() except KeyError as e: print "Could not start log configuration," \ "{} not found in TOC".format(str(e)) except AttributeError: print "Could not add Stabilizer log config, bad configuration." while True: self._cf.param.set_value("flightmode.althold", "True") self._cf.commander.send_setpoint(Drone.roll, Drone.pitch, Drone.yaw, thrust) # thrust = base_thrust_value # if self.thrust_flag: # self._cf.param.set_value("flightmode.althold", "False") """ if thrust < 65000 and Drone.thruster == 1: thrust += thrust_step * Drone.thruster Drone.thruster = 0 elif thrust + thrust_step * Drone.thruster > 35000 and Drone.thruster == -1: thrust += thrust_step * Drone.thruster Drone.thruster = 0 """ if 35000 < thrust + thrust_step * Drone.thruster <= 65000: thrust += thrust_step * Drone.thruster Drone.thruster = 0 Drone.thruster = 0 self.thrust_flag = False # else: # self._cf.param.set_value("flightmode.althold", "True") Drone.pitch = Drone.roll = 0 # time.sleep(0.1) self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self._cf.close_link() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print "Log: [%d][%s]: %s" % (timestamp, logconf.name, data) print(data['stabilizer.roll']) self.roll = -data['stabilizer.roll'] self.pitch = -data['stabilizer.pitch'] self.yaw = -data['stabilizer.yaw'] f_thrust = open('data_log_thrust.txt', 'a') f_roll = open('data_log_roll.txt', 'a') f_pitch = open('data_log_pitch.txt', 'a') f_yaw = open('data_log_yaw.txt', 'a') f_data = open('data_log.txt', 'a') f_thrust.write(str(data['stabilizer.thrust']) + ",") f_roll.write(str(data['stabilizer.roll']) + ",") f_pitch.write(str(data['stabilizer.pitch']) + ",") f_yaw.write(str(data['stabilizer.yaw']) + ",") f_data.write( str(data['stabilizer.thrust']) + "," + str(data['stabilizer.roll']) + "," + str(data['stabilizer.pitch']) + "," + str(data['stabilizer.yaw']) + "\n") f_roll.close() f_pitch.close() f_yaw.close() f_thrust.close() f_data.close()
class CrazyflieControl: """ Class that connects to a Crazyflie and controls it until disconnection. """ #------------------------------------------------------------------------------ # Initialization #------------------------------------------------------------------------------ def __init__(self, link_uri): """ Initializes the control class and executes all needed functions. """ # Connect Crazyflie self.Crazyflie = Crazyflie() self.Connected = False self.Connect(link_uri) while not self.Connected: wait(0.1) pass # Start Program self.t0 = 0#getTime() self.Running = True # Initialize self.SetInitialState() self.InitializeReferenceCS() if Plotting: self.InitializePlotting() if GUI: self.InitializeGUI() if Animation: self.InitializeAnimation() # Run Main Loops Thread(target=self.MainLoop).start() if GUI: Thread(target=self.GUILoop).start() if Animation: Thread(target=self.AnimationLoop).start() def SetInitialState(self): ''' Sets all initial control variables to be used later on. ''' # Intial Values self.thrust_mult = 1 self.thrust_step = 100#250#500 # Lift-off at approx. 45000 self.thrust = 25e3#35e3#25e3#35e3#40e3#36e3#22e3#-#50e3#48e3#45e3#35e3#30e3#20e3 self.thrustmin = 23e3#30e3#23e3#30e3#38e3#30e3#20e3#-#30e3 self.thrustmax = 26e3#38e3#26e3#38e3#42e3#38e3#30e3#-#55e3#50e3#45e3#40e3#38e3#35e3#3e30#45e3#25e3 self.pitch = 0 self.roll = 0 self.yawrate = 0 # self.thrust_mult = thrust_mult # self.thrust_step = thrust_step # self.thrust = thrust # self.thrustmin = thrustmin # self.thrustmax = thrustmax # self.pitch = pitch # self.roll = roll # self.yawrate = yawrate self.roll_Real = 0. self.pitch_Real = 0. self.yaw_Real = 0. self.att = np.zeros(3) self.pos = np.zeros(3) self.vel = np.zeros(3) self.a = np.zeros(3) self.velRel = np.zeros(3) self.aRel = np.zeros(3) self.aRaw = np.zeros(3) self.a_DataTimer = 0 self.baro = 0. self.baro_0 = 0. self.battery = 0. self.motors = np.zeros(4) self.gyro = np.zeros(3) self.mag = np.zeros(3) PrintInfo("Initial Values Set") def InitializeReferenceCS(self): ''' Initializes the Coordinate Systems of the Crazyflie ''' self.refCS = np.array([\ [0,1,0],\ [-1,0,0],\ [0,0,1]\ ]) self.CS0 = CoordinateSystem(refCS=self.refCS,RotationSigns=[1,1,-1],name="Default Coordinate System") self.CS = CoordinateSystem(refCS=self.refCS,RotationSigns=[1,1,-1],name="Crazyflie Coordinate System") PrintInfo("Reference Systems initialized") def InitializeGUI(self): ''' Initializes the GUI for the state representation and user interaction ''' self.InitStateWindow() self.CrazyflieDataText="" PrintInfo("GUI initialized") def InitializeAnimation(self): ''' Initializes the Animation Window and the 3D Model of the Crazyflie ''' self.Model3D = None self.Window = Window() self.Create_3DModel() PrintInfo("Animation initialized") #------------------------------------------------------------------------------ # Crazyflie Connection Status Callbacks #------------------------------------------------------------------------------ def Connect(self,link_uri): self.Crazyflie.connected.add_callback(self.connected) self.Crazyflie.disconnected.add_callback(self.disconnected) self.Crazyflie.connection_failed.add_callback(self.connection_failed) self.Crazyflie.connection_lost.add_callback(self.connection_lost) self.Crazyflie.open_link(link_uri) 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.MainLoop).start() self.Connected = True PrintInfo("Connection to %s is established" % link_uri) def connection_failed(self, link_uri, msg): """ Callback when connection initial connection fails (i.e no Crazyflie at the speficied address) """ PrintError("Connection to %s failed: %s" % (link_uri, msg)) def connection_lost(self, link_uri, msg): """ Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range) """ PrintError("Connection to %s lost: %s" % (link_uri, msg)) def disconnected(self, link_uri): """ Callback when the Crazyflie is disconnected (called in all cases) """ PrintError("Disconnected from %s" % link_uri) #------------------------------------------------------------------------------ # Main Loop #------------------------------------------------------------------------------ def MainLoop(self): self.run = 0 self.SetCalibratedState() self.InitializeLogging() self.InitializeGetParameters() self.ControlLoop() self.Kill() def Kill(self): print ">>>Killing Drone<<<" self.Crazyflie.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing self.Running = False wait(0.1) self.Crazyflie.close_link() def KillCheck(self): if abs(self.att[0])>=70 or abs(self.att[1])>=70: print ">Angle Kill<" self.Running=False if getKey("Escape")!=0: print ">Escape Kill<" self.Running=False # if self.thrust <= self.thrustmin: # print ">ThrustMin Kill<" # self.Running=False #------------------------------------------------------------------------------ # Personal Control #------------------------------------------------------------------------------ def ControlLoop(self): while self.Running: self.run+=1 # print "Run #"+str(self.run) self.GetState() #self.PrintState() self.GetInfoText() self.SendState() self.EvaluateData() self.KillCheck() def GUILoop(self): while self.Running: if True: # try: self.StateWindow.fill([0]*3) lines = self.CrazyflieDataText.split("\n") for i in range(len(lines)): line = lines[i] render_text(self.StateWindow,line,[300,100+i*30],self.Font) pg.display.flip() pg.event.wait() # except: # PrintSpecial("Display Failed!","Red") else: self.PrintData() def AnimationLoop(self): if self.Model3D!=None: self.Model3D.Update() def SetCalibratedState(self): if Calibrate: # Positive: Front || Negative: Back self.pitch = 4#5#-#40#38#35#32#28#-28 # Positive: Right || Negative: Left self.roll = 0#-#-22#-20#-18#-15#-10#-2 def GetState(self): # Climb & Decent if self.thrust >= self.thrustmax: self.thrust_mult = 0#-1 self.thrust += self.thrust_step * self.thrust_mult self.thrust=0 if Stabilize: # Stabilize if self.roll_Real<self.roll: self.roll+=1. elif self.roll_Real>self.roll: self.roll-=1. if self.pitch_Real<self.pitch: self.roll+=1. elif self.pitch_Real>self.pitch: self.pitch-=1. def SendState(self): self.Crazyflie.commander.send_setpoint(self.roll,self.pitch,self.yawrate,self.thrust) wait(0.1) def PrintState(self): print "-"*30 print "Set Data:" print PH+"Thrust: "+str(round(self.thrust,2)) print PH+"Roll: "+str(round(self.roll,2)) print PH+"Pitch: "+str(round(self.pitch,2)) print PH+"Yaw: "+str(round(self.yawrate,2)) #------------------------------------------------------------------------------ # Logging #------------------------------------------------------------------------------ def EvaluateData(self): self.att = np.array([self.roll_Real,-self.pitch_Real,-self.yaw_Real]) # Update CS self.CS.setCS(self.att) if Plotting: if self.t0==0: self.t0=getTime() # print "Timestamp: ",timestamp t = getTime()-self.t0 self.Stabilizer_Plot.UpdateGraph(graphname="Roll" ,pos=[t,np.degrees(self.roll_Real)]) self.Stabilizer_Plot.UpdateGraph(graphname="Pitch",pos=[t,np.degrees(self.pitch_Real)]) self.Stabilizer_Plot.UpdateGraph(graphname="Yaw" ,pos=[t,np.degrees(self.yaw_Real)]) self.Velocity_Plot.UpdateGraph(graphname="X" ,pos=[t,self.vel[0]]) self.Velocity_Plot.UpdateGraph(graphname="Y" ,pos=[t,self.vel[1]]) self.Velocity_Plot.UpdateGraph(graphname="Z" ,pos=[t,self.vel[2]]) #print self.aRaw[2],VectorAbs(self.aRaw)#self.a self.Acceleration_Plot.UpdateGraph(graphname="X_Earth",pos=[t,self.a[0]]) self.Acceleration_Plot.UpdateGraph(graphname="Y_Earth",pos=[t,self.a[1]]) self.Acceleration_Plot.UpdateGraph(graphname="Z_Earth",pos=[t,self.a[2]]) self.Acceleration_Plot.UpdateGraph(graphname="X_Raw",pos=[t,self.aRaw[0]]) self.Acceleration_Plot.UpdateGraph(graphname="Y_Raw",pos=[t,self.aRaw[1]]) self.Acceleration_Plot.UpdateGraph(graphname="Z_Raw",pos=[t,self.aRaw[2]]) def InitializePlotting(self): self.Stabilizer_Plot = Debug_Display("Attitude",u"°",x=0,y=0,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=5,IndicationRange=20) self.Stabilizer_Plot.addGraph(name="Roll" ,color=Colors["Red"] ,label=False) self.Stabilizer_Plot.addGraph(name="Pitch",color=Colors["Green"],label=False) self.Stabilizer_Plot.addGraph(name="Yaw" ,color=Colors["Blue"] ,label=False) self.Velocity_Plot = Debug_Display("Velocity",u"m/s",x=0,y=SysInfo.Resolution[1]/3,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=2,IndicationRange=8) self.Velocity_Plot.addGraph(name="X",color=Colors["Red"] ,label=False) self.Velocity_Plot.addGraph(name="Y",color=Colors["Green"],label=False) self.Velocity_Plot.addGraph(name="Z",color=Colors["Blue"] ,label=False) self.Acceleration_Plot = Debug_Display("Acceleration",u"m/s²",x=0,y=SysInfo.Resolution[1]*2/3,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=2,IndicationRange=8) self.Acceleration_Plot.addGraph(name="X_Earth",color=Colors["Red"] ,label=False) self.Acceleration_Plot.addGraph(name="Y_Earth",color=Colors["Green"],label=False) self.Acceleration_Plot.addGraph(name="Z_Earth",color=Colors["Blue"] ,label=False) self.Acceleration_Plot.addGraph(name="X_Raw",color=Colors["Red"] *0.5,label=False) self.Acceleration_Plot.addGraph(name="Y_Raw",color=Colors["Green"]*0.5,label=False) self.Acceleration_Plot.addGraph(name="Z_Raw",color=Colors["Blue"] *0.5,label=False) def InitializeLogging(self): # Attitude (if period_in_ms = <10 -> Invalid -> other Logs start working again?! O.o) self.StabilizerLog = LogConfig(name="Stabilizer", period_in_ms=10) self.StabilizerLog.add_variable("stabilizer.roll", "float") self.StabilizerLog.add_variable("stabilizer.pitch", "float") self.StabilizerLog.add_variable("stabilizer.yaw", "float") self.Crazyflie.log.add_config(self.StabilizerLog) # Battery self.BatteryLog = LogConfig(name="Battery", period_in_ms=200) self.BatteryLog.add_variable("pm.vbat", "float") self.Crazyflie.log.add_config(self.BatteryLog) # Barometer self.BarometerLog = LogConfig(name="Barometer", period_in_ms=10) self.BarometerLog.add_variable("baro.aslLong", "float") self.Crazyflie.log.add_config(self.BarometerLog) # Accelerometer (if period_in_ms = <10 -> Invalid -> other Logs start working again?! O.o) self.AccelerometerLog = LogConfig(name="Accelerometer", period_in_ms=10) self.AccelerometerLog.add_variable("acc.x", "float") self.AccelerometerLog.add_variable("acc.y", "float") self.AccelerometerLog.add_variable("acc.z", "float") self.Crazyflie.log.add_config(self.AccelerometerLog) # Motors self.MotorsLog = LogConfig(name="Motors", period_in_ms=50) self.MotorsLog.add_variable("motor.m1", "int32_t") self.MotorsLog.add_variable("motor.m2", "int32_t") self.MotorsLog.add_variable("motor.m3", "int32_t") self.MotorsLog.add_variable("motor.m4", "int32_t") self.Crazyflie.log.add_config(self.MotorsLog) # Gyro self.GyroLog = LogConfig(name="Gyro", period_in_ms=50) self.GyroLog.add_variable("gyro.x", "float") self.GyroLog.add_variable("gyro.y", "float") self.GyroLog.add_variable("gyro.z", "float") self.Crazyflie.log.add_config(self.GyroLog) # Magnetometer self.MagnetometerLog = LogConfig(name="Magnetometer", period_in_ms=50) self.MagnetometerLog.add_variable("mag.x", "float")#int16_t") self.MagnetometerLog.add_variable("mag.y", "float")#int16_t") self.MagnetometerLog.add_variable("mag.z", "float")#int16_t") self.Crazyflie.log.add_config(self.MagnetometerLog) #wait(10) if self.StabilizerLog.valid: # This callback will receive the data self.StabilizerLog.data_received_cb.add_callback(self.Stabilizer_log_data) # This callback will be called on errors self.StabilizerLog.error_cb.add_callback(self.log_error) # Start the logging self.StabilizerLog.start() else: print("Could not add logconfig '"+str(self.StabilizerLog.name)+"' since some variables are not in TOC") wait(2) if self.BatteryLog.valid: # This callback will receive the data self.BatteryLog.data_received_cb.add_callback(self.Battery_log_data) # This callback will be called on errors self.BatteryLog.error_cb.add_callback(self.log_error) # Start the logging self.BatteryLog.start() else: print("Could not add logconfig '"+str(self.BatteryLog.name)+"' since some variables are not in TOC") wait(2) if self.BarometerLog.valid: # This callback will receive the data self.BarometerLog.data_received_cb.add_callback(self.Barometer_log_data) # This callback will be called on errors self.BarometerLog.error_cb.add_callback(self.log_error) # Start the logging self.BarometerLog.start() else: print("Could not add logconfig '"+str(self.BarometerLog.name)+"' since some variables are not in TOC") wait(2) if self.AccelerometerLog.valid: # This callback will receive the data self.AccelerometerLog.data_received_cb.add_callback(self.Accelerometer_log_data) # This callback will be called on errors self.AccelerometerLog.error_cb.add_callback(self.log_error) # Start the logging self.AccelerometerLog.start() else: print("Could not add logconfig '"+str(self.AccelerometerLog.name)+"' since some variables are not in TOC") wait(2) if self.MotorsLog.valid: # This callback will receive the data self.MotorsLog.data_received_cb.add_callback(self.Motors_log_data) # This callback will be called on errors self.MotorsLog.error_cb.add_callback(self.log_error) # Start the logging self.MotorsLog.start() else: print("Could not add logconfig '"+str(self.MotorsLog.name)+"' since some variables are not in TOC") wait(2) if self.GyroLog.valid: # This callback will receive the data self.GyroLog.data_received_cb.add_callback(self.Gyro_log_data) # This callback will be called on errors self.GyroLog.error_cb.add_callback(self.log_error) # Start the logging self.GyroLog.start() else: print("Could not add logconfig '"+str(self.GyroLog.name)+"' since some variables are not in TOC") wait(2) if self.MagnetometerLog.valid: # This callback will receive the data self.MagnetometerLog.data_received_cb.add_callback(self.Magnetometer_log_data) # This callback will be called on errors self.MagnetometerLog.error_cb.add_callback(self.log_error) # Start the logging self.MagnetometerLog.start() else: print("Could not add logconfig '"+str(self.MagnetometerLog.name)+"' since some variables are not in TOC") wait(2) def Stabilizer_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" #print "[%d][%s]: %s" % (timestamp, logconf.name, data) self.roll_Real = np.radians(data['stabilizer.roll']) self.pitch_Real = np.radians(data['stabilizer.pitch']) self.yaw_Real = np.radians(data['stabilizer.yaw']) def Battery_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" self.battery = data['pm.vbat'] def Barometer_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" if self.baro_0==0: self.baro_0 = copy(data['baro.aslLong']) self.baro = data['baro.aslLong']-self.baro_0 def Accelerometer_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" if self.a_DataTimer==0: self.a_DataTimer = getTime() dt = getTime()-self.a_DataTimer self.a_DataTimer = getTime() self.aRaw = np.array([\ data['acc.x'],\ data['acc.y'],\ data['acc.z']\ ]).astype(float)*10#/1000 # self.EvaluateAccelerometerData() # def EvaluateAccelerometerData(self): self.aRel = copy(self.aRaw) #print self.aRel Gravity = 9.81 relative_Gravity = [0,0,-Gravity] relative_Gravity = EulerRotation(relative_Gravity,[1,0,0],self.roll_Real) relative_Gravity = EulerRotation(relative_Gravity,[0,1,0],self.pitch_Real) self.aRel+=relative_Gravity # Body_to_Earth = rotation_matrix([1,0,0],self.att[0],matrix=True)*rotation_matrix([0,1,0],self.att[1],matrix=True)*rotation_matrix([0,0,1],self.att[2],matrix=True) self.a=self.aRel#self.a = np.dot(Body_to_Earth,self.aRel) self.velRel += self.aRel*dt self.pos += self.velRel*dt #print self.pos def Motors_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" self.motors = np.array([\ data["motor.m1"],\ data["motor.m2"],\ data["motor.m3"],\ data["motor.m4"]\ ]) def Gyro_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" self.gyro = np.array([\ data['gyro.x'],\ data['gyro.y'],\ data['gyro.z']\ ]) def Magnetometer_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives""" self.mag = [\ data['mag.x'],\ data['mag.y'],\ data['mag.z']\ ] def log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) def PrintData(self): print self.CrazyflieDataText def GetInfoText(self): self.CrazyflieDataText = "" self.CrazyflieDataText += "-"*30+"\n" self.CrazyflieDataText += "Received Data:"+"\n" self.CrazyflieDataText += PH+"Roll: "+str(round(np.degrees(self.att[0]),3))+u" [°]"+"\n"#roll_Real ),3))+u" [°]"+"\n" self.CrazyflieDataText += PH+"Pitch: "+str(round(np.degrees(self.att[1]),3))+u" [°]"+"\n"#pitch_Real),3))+u" [°]"+"\n" self.CrazyflieDataText += PH+"Yaw: "+str(round(np.degrees(self.att[2]),3))+u" [°]"+"\n"#yaw_Real ),3))+u" [°]"+"\n" self.CrazyflieDataText += "\n" self.CrazyflieDataText += PH+"Accelerometer: ["+str(round(self.a [0],3))+","+str(round(self.a [1],3))+","+str(round(self.a [2],3))+"] ("+str(round(VectorAbs(self.a ),3))+")"+u" [m/s²]"+"\n" self.CrazyflieDataText += PH+"Velocity: ["+str(round(self.vel[0],3))+","+str(round(self.vel[1],3))+","+str(round(self.vel[2],3))+"] ("+str(round(VectorAbs(self.vel),3))+")"+u" [m/s]"+"\n" self.CrazyflieDataText += PH+"Position: ["+str(round(self.pos[0],3))+","+str(round(self.pos[1],3))+","+str(round(self.pos[2],3))+"] ("+str(round(VectorAbs(self.pos),3))+")"+u" [m]"+"\n" self.CrazyflieDataText += "\n" self.CrazyflieDataText += PH+"Barometer: "+str(round(self.baro ,3))+u" [m]" +" || Barometer_0: "+str(round(self.baro_0,3))+u" [m]" +"\n"+"\n" self.CrazyflieDataText += PH+"Battery: "+str(round(self.battery*10**3,3))+u" [mV]"+"\n" self.CrazyflieDataText += PH+"Motors: "+\ str(round(self.motors[0],3))+","+\ str(round(self.motors[1],3))+","+\ str(round(self.motors[2],3))+","+\ str(round(self.motors[3],3))+\ u" [?]" +"\n" self.CrazyflieDataText += PH+"Gyro: "+str(round(self.gyro[0],3))+","+str(round(self.gyro[1],3))+","+str(round(self.gyro[2],3))+u" [?]" +"\n" self.CrazyflieDataText += PH+"Magneto: "+str(round(self.mag[0] ,3))+","+str(round(self.mag[1] ,3))+","+str(round(self.mag[2] ,3))+u" [?]"+"\n" # print "-"*30 # print "Received Data:" # print PH+"Roll: "+str(round(self.roll_Real ,3))+u" [°]" # print PH+"Pitch: "+str(round(self.pitch_Real ,3))+u" [°]" # print PH+"Yaw: "+str(round(self.yaw_Real ,3))+u" [°]" # # print # # print PH+"Accelerometer: "+str(np.round(self.a,3))+u" [m/s²]" # # print # # print PH+"Baro: "+str(round(self.baro ,3))+u" [m]" # # print PH+"Battery: "+str(round(self.battery*10**3,3))+u" [mV]" # # print PH+"Motors: "+str(np.round(self.motors ,3))+u" [?]" # # print PH+"Gyro: "+str(np.round(self.gyro ,3))+u" [?]" # # print PH+"Magneto: s"+str(np.round(self.mag ,3))+u" [?]" #------------------------------------------------------------------------------ # Get Parameters #------------------------------------------------------------------------------ def InitializeGetParameters(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 # Variable used to keep main loop occupied until disconnect self.is_connected = True self.param_check_list = [] self.param_groups = [] random.seed() # Print the param TOC self.CrazyflieParameters = dict() p_toc = self.Crazyflie.param.toc.toc for group in sorted(p_toc.keys()): Group = "{}".format(group) #print Group # <------------------------------------------------------------------------------- self.CrazyflieParameters[Group] = dict() for param in sorted(p_toc[group].keys()): Parameter = "{}".format(param) #print "\t"+Parameter # <------------------------------------------------------------------------------- self.CrazyflieParameters[Group][Parameter] = 1 self.param_check_list.append("{0}.{1}".format(group, param)) self.param_groups.append("{}".format(group)) # For every group, register the callback self.Crazyflie.param.add_update_callback(group=group, name=None, cb=self.param_callback) #self.PrintParameters() # # You can also register a callback for a specific group.name combo # self.Crazyflie.param.add_update_callback(group="cpu", name="flash", # cb=self.cpu_flash_callback) print print "Reading back all parameter values" # Request update for all the parameters using the full name # group.name for p in self.param_check_list: self.Crazyflie.param.request_param_update(p) # # Set LED-Ring Test # wait(20) # Color = [0,0,40] # print "LED Ring effect changed to Color :"+str(Color) # self.SetLED_RGB(Color) # ringeffect = 1 # print "LED Ring effect changed to Mode #"+str(ringeffect) # #self.Crazyflie.param.set_value("ring.effect","{:.2f}".format(ringeffect)) # SetParameter("ring","effect","1") def SetParameter(self,Group,Parameter,Value): # print "Set Parameter:",Group+"."+Parameter,"{:.2f}".format(Value) # self.Crazyflie.param.set_value(Group+"."+Parameter,"{:.2f}".format(Value)) print "Set Parameter:",Group+"."+Parameter,"{:2}".format(Value) self.Crazyflie.param.set_value(Group+"."+Parameter,"{:2}".format(Value)) def SetLED_RGB(self,Color): self.SetParameter("ring","effect",str(7)) self.SetParameter("ring","solidRed" ,str(Color[0])) self.SetParameter("ring","solidGreen",str(Color[1])) self.SetParameter("ring","solidBlue" ,str(Color[2])) print "LED color changed to: "+str(Color) def PrintParameters(self): print ">>>Crazyflie Parameters<<<" for Group in self.CrazyflieParameters: print Group for Parameter in self.CrazyflieParamet,ers[Group]: print PH+"- "+StringWithBlankspace(Parameter,20)+": "+str(self.CrazyflieParameters[Group][Parameter]) def cpu_flash_callback(self, name, value): """Specific callback for the cpu.flash parameter""" print "The connected Crazyflie has {}kb of flash".format(value) def param_callback(self, name, value): """Generic callback registered for all the groups""" print "{0}: {1}".format(name, value) # <------------------------------------------------------------------------------- # Group,Parameter=name.split(".") # print ">",Group,Name,Value # self.CrazyflieParameters[Group][Parameter] = value Categories = name.split(".") Group,Parameter = Categories[0],Categories[1] #print ">",Group,Parameter,value#,Name,Value self.CrazyflieParameters[Group][Parameter] = value self.Crazyflie.param.add_update_callback(group=Group,name=Paramater,cb=self.parameter_update_callback) # Remove each parameter from the list and close the link when # all are fetched self.param_check_list.remove(name) if len(self.param_check_list) == 0: print ">>>Have fetched all parameter values." # First remove all the group callbacks for g in self.param_groups: self.Crazyflie.param.remove_update_callback(group=g,cb=self.param_callback) # # Create a new random value [0.00,1.00] for pid_attitude.pitch_kd # # and set it # pkd = random.random() # print # print "Write: pid_attitude.pitch_kd={:.2f}".format(pkd) # self.Crazyflie.param.add_update_callback(group="pid_attitude", # name="pitch_kd", # cb=self.a_pitch_kd_callback) # # When setting a value the parameter is automatically read back # # and the registered callbacks will get the updated value # self.Crazyflie.param.set_value("pid_attitude.pitch_kd", # "{:.2f}".format(pkd)) self._cf.param.add_update_callback(group="ring",name="effect",cb=self._a_ring_effect_callback) self._cf.param.set_value("ring.effect","1") self.PrintParameters() # Set LED-Ring Test Color = [0,0,40] print "LED Ring effect changed to Color :"+str(Color) self.SetLED_RGB(Color) def _a_ring_effect_callback(self, name, value): """Callback for ring_effect""" print "Readback (LED_effect): {0}={1}".format(name, value) print def parameter_update_callback(self,name,value): print "Readback: {0}={1}".format(name, value) #------------------------------------------------------------------------------ # 3D-Model #------------------------------------------------------------------------------ def Create_3DModel(self): self.Model3D = Crazyflie3D(self) def InitStateWindow(self): init_pygame() pos = [0,0]#[100,100]#[0,0] resolution = [SysInfo.Resolution[0]/4,SysInfo.Resolution[1]] #PrintSpecial("Opening State Window","Red") self.StateWindow = scr_init(pos=pos,reso=resolution,fullscreen=False,frameless=False,mouse=True,caption="Crazyflie Status")#,icon="Instrument Icon.png") #PrintSpecial("State Window Opened","Red") font='LCD.TTF' self.Font = pg.font.SysFont(font,30)#'fonts/'+
class Main: # Initial values, you can use these to set trim etc. roll = 0.0 pitch = 0.0 yaw = 0 thrust = 15000 def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() dev = sys.argv[1] print 'device tag: %s' % dev # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/" + dev + "/2M") self.crazyflie.connected.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() print "Now accepting input\n>" while 1: val = getch.getch() if val == 'w': self.pitchUp() elif val == 's': self.pitchDown() elif val == 'a': self.rollDown() elif val == 'd': self.rollUp() elif val == 'q': self.yawUp() elif val == 'e': self.yawDown() elif val == 'o': self.thrustDown() elif val == 'p': self.thrustUp() elif val == 'x': self.shutdown() def pulse_command(self): while True: self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) time.sleep(0.5) def pitchUp(self): self.pitch = self.pitch + 1 print 'pitch up', self.pitch def pitchDown(self): self.pitch = self.pitch - 1 print 'pitch down', self.pitch def rollUp(self): self.roll = self.roll + 1 print 'roll up', self.roll def rollDown(self): self.roll = self.roll - 1 print 'roll down', self.roll def yawUp(self): self.yaw = self.yaw + 1 print 'yaw up', self.yaw def yawDown(self): self.yaw = self.yaw - 1 print 'yaw down', self.yaw def thrustUp(self): self.thrust = self.thrust + 1000 if(self.thrust > 60000): self.thrust = 60000 print 'thrust up', self.thrust def thrustDown(self): self.thrust = self.thrust - 1000 if(self.thrust < 10000): self.thrust = 10000 print 'thrust down', self.thrust def shutdown(self): self.thrust = 10000 self.yaw = 0 self.roll = 0 self.pitch = 0 print "Resetting controls and closing link" self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) self.crazyflie.close_link()
class SyncCrazyflie: def __init__(self, link_uri, cf=None): """ Create a synchronous Crazyflie instance with the specified link_uri """ if cf: self.cf = cf else: self.cf = Crazyflie() self._link_uri = link_uri self._connect_event = None self._disconnect_event = None self._is_link_open = False self._error_message = None def open_link(self): if (self.is_link_open()): raise Exception('Link already open') self._add_callbacks() logger.debug('Connecting to %s' % self._link_uri) self._connect_event = Event() self.cf.open_link(self._link_uri) self._connect_event.wait() self._connect_event = None if not self._is_link_open: self._remove_callbacks() raise Exception(self._error_message) def __enter__(self): self.open_link() return self def close_link(self): if (self.is_link_open()): self._disconnect_event = Event() self.cf.close_link() self._disconnect_event.wait() self._disconnect_event = None def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() def is_link_open(self): return self._is_link_open 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.""" logger.debug('Connected to %s' % link_uri) self._is_link_open = True if self._connect_event: self._connect_event.set() def _connection_failed(self, link_uri, msg): """Callback when initial connection fails (i.e no Crazyflie at the specified address)""" logger.debug('Connection to %s failed: %s' % (link_uri, msg)) self._is_link_open = False self._error_message = msg if self._connect_event: self._connect_event.set() def _disconnected(self, link_uri): self._remove_callbacks() self._is_link_open = False if self._disconnect_event: self._disconnect_event.set() def _add_callbacks(self): self.cf.connected.add_callback(self._connected) self.cf.connection_failed.add_callback(self._connection_failed) self.cf.disconnected.add_callback(self._disconnected) def _remove_callbacks(self): def remove_callback(container, callback): try: container.remove_callback(callback) except ValueError: pass remove_callback(self.cf.connected, self._connected) remove_callback(self.cf.connection_failed, self._connection_failed) remove_callback(self.cf.disconnected, self._disconnected)
class MainUI(QtWidgets.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) # Restore window size if present in the config file try: size = Config().get("window_size") self.resize(size[0], size[1]) except KeyError: pass ###################################################### # By lxrocks # 'Skinny Progress Bar' tweak for Yosemite # Tweak progress bar - artistic I am not - so pick your own colors !!! # Only apply to Yosemite ###################################################### import platform if platform.system() == 'Darwin': (Version, junk, machine) = platform.mac_ver() logger.info("This is a MAC - checking if we can apply Progress " "Bar Stylesheet for Yosemite Skinny Bars ") yosemite = (10, 10, 0) tVersion = tuple(map(int, (Version.split(".")))) if tVersion >= yosemite: logger.info("Found Yosemite - applying stylesheet") tcss = """ QProgressBar { border: 1px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: """ + COLOR_BLUE + """; } """ self.setStyleSheet(tcss) else: logger.info("Pre-Yosemite - skinny bar stylesheet not applied") ###################################################### self.cf = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.crtp.init_drivers(enable_debug_driver=Config() .get("enable_debug_driver")) zmq_params = ZMQParamAccess(self.cf) zmq_params.start() zmq_leds = ZMQLEDDriver(self.cf) zmq_leds.start() self.scanner = ScannerThread() self.scanner.interfaceFoundSignal.connect(self.foundInterfaces) self.scanner.start() # Create and start the Input Reader self._statusbar_label = QLabel("No input-device found, insert one to" " fly.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" # self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) # TODO: Need to reload configs # ConfigManager().conf_needs_reload.add_callback(self._reload_configs) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect( self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Hide the 'File' menu on OS X, since its only item, 'Exit', gets # merged into the application menu. if sys.platform == 'darwin': self.menuFile.menuAction().setVisible(False) # Connect UI signals self.logConfigAction.triggered.connect(self._show_connect_dialog) self.interfaceCombo.currentIndexChanged['QString'].connect( self.interfaceChanged) self.connectButton.clicked.connect(self._connect) self.scanButton.clicked.connect(self._scan) self.menuItemConnect.triggered.connect(self._connect) self.menuItemConfInputDevice.triggered.connect( self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_battery) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self._disable_input = False self.joystickReader.input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander.send_setpoint(*args)) self.joystickReader.assisted_input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander.send_velocity_world_setpoint(*args)) self.joystickReader.heighthold_input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander.send_zdistance_setpoint(*args)) self.joystickReader.hover_input_updated.add_callback( self.cf.commander.send_hover_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self._disconnected) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(self._connection_initiated) self._log_error_signal.connect(self._logging_error) self.batteryBar.setTextVisible(False) self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) self.linkQualityBar.setTextVisible(False) self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) self._selected_interface = None self._initial_scan = True self._scan() # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader cfclient.ui.pluginhelper.mainUI = self self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show) # Load and connect tabs self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True) self.menuView.addMenu(self.tabsMenuItem) # self.tabsMenuItem.setMenu(QtWidgets.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t is not None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView, enabled=True) self.menuView.addMenu(self.toolboxesMenuItem) self.toolboxes = [] for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtWidgets.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # References to all the device sub-menus in the "Input device" menu self._all_role_menus = () # Used to filter what new devices to add default mapping to self._available_devices = () # Keep track of mux nodes so we can enable according to how many # devices we have self._all_mux_nodes = () # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m.name, self._menu_inputdevice, checkable=True, enabled=False) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_inputdevice.addAction(node) self._all_mux_nodes += (node,) mux_subnodes = () for name in m.supported_roles(): sub_node = QMenu(" {}".format(name), self._menu_inputdevice, enabled=False) self._menu_inputdevice.addMenu(sub_node) mux_subnodes += (sub_node,) self._all_role_menus += ({"muxmenu": node, "rolemenu": sub_node},) node.setData((m, mux_subnodes)) self._mapping_support = True def disable_input(self, disable): """ Disable the gamepad input to be able to send setpoint from a tab """ self._disable_input = disable def interfaceChanged(self, interface): if interface == INTERFACE_PROMPT_TEXT: self._selected_interface = None else: self._selected_interface = interface self._update_ui_state() def foundInterfaces(self, interfaces): selected_interface = self._selected_interface self.interfaceCombo.clear() self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT) formatted_interfaces = [] for i in interfaces: if len(i[1]) > 0: interface = "%s - %s" % (i[0], i[1]) else: interface = i[0] formatted_interfaces.append(interface) self.interfaceCombo.addItems(formatted_interfaces) if self._initial_scan: self._initial_scan = False try: if len(Config().get("link_uri")) > 0: formatted_interfaces.index(Config().get("link_uri")) selected_interface = Config().get("link_uri") except KeyError: # The configuration for link_uri was not found pass except ValueError: # The saved URI was not found while scanning pass if len(interfaces) == 1 and selected_interface is None: selected_interface = interfaces[0][0] newIndex = 0 if selected_interface is not None: try: newIndex = formatted_interfaces.index(selected_interface) + 1 except ValueError: pass self.interfaceCombo.setCurrentIndex(newIndex) self.uiState = UIState.DISCONNECTED self._update_ui_state() def _update_ui_state(self): if self.uiState == UIState.DISCONNECTED: self.setWindowTitle("Not connected") canConnect = self._selected_interface is not None self.menuItemConnect.setText("Connect to Crazyflie") self.menuItemConnect.setEnabled(canConnect) self.connectButton.setText("Connect") self.connectButton.setToolTip( "Connect to the Crazyflie on the selected interface") self.connectButton.setEnabled(canConnect) self.scanButton.setText("Scan") self.scanButton.setEnabled(True) self.address.setEnabled(True) self.batteryBar.setValue(3000) self._menu_cf2_config.setEnabled(False) self._menu_cf1_config.setEnabled(True) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) self.interfaceCombo.setEnabled(True) elif self.uiState == UIState.CONNECTED: s = "Connected on %s" % self._selected_interface self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Disconnect") self.connectButton.setToolTip("Disconnect from the Crazyflie") self.scanButton.setEnabled(False) self.logConfigAction.setEnabled(True) # Find out if there's an I2C EEPROM, otherwise don't show the # dialog. if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0: self._menu_cf2_config.setEnabled(True) self._menu_cf1_config.setEnabled(False) elif self.uiState == UIState.CONNECTING: s = "Connecting to {} ...".format(self._selected_interface) self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Cancel") self.connectButton.setToolTip("Cancel connecting to the Crazyflie") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) elif self.uiState == UIState.SCANNING: self.setWindowTitle("Scanning ...") self.connectButton.setText("Connect") self.menuItemConnect.setEnabled(False) self.connectButton.setText("Connect") self.connectButton.setEnabled(False) self.scanButton.setText("Scanning...") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() # for c in self._menu_mappings.actions(): # c.setEnabled(False) # devs = self.joystickReader.available_devices() # if (len(devs) > 0): # self.device_discovery(devs) def _show_input_device_config_dialog(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked Config().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: {}".format(checked)) def _show_connect_dialog(self): self.logConfigDialogue.show() def _update_battery(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) color = COLOR_BLUE # TODO firmware reports fully-charged state as 'Battery', # rather than 'Charged' if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]: color = COLOR_GREEN elif data["pm.state"] == BatteryStates.LOW_POWER: color = COLOR_RED self.batteryBar.setStyleSheet(progressbar_stylesheet(color)) def _connected(self): self.uiState = UIState.CONNECTED self._update_ui_state() Config().set("link_uri", str(self._selected_interface)) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") lg.add_variable("pm.state", "int8_t") try: self.cf.log.add_config(lg) lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() except KeyError as e: logger.warning(str(e)) mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED) if len(mems) > 0: mems[0].write_data(self._led_write_done) def _disconnected(self): self.uiState = UIState.DISCONNECTED self._update_ui_state() def _connection_initiated(self): self.uiState = UIState.CONNECTING self._update_ui_state() def _led_write_done(self, mem, addr): logger.info("LED write done callback") def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [{}]: {}".format(log_conf.name, msg)) def _connection_lost(self, linkURI, msg): if not self._auto_reconnect_enabled: if self.isActiveWindow(): warningCaption = "Communication failure" error = "Connection lost to {}: {}".format(linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def _connection_failed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on {}: {}".format(linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def resizeEvent(self, event): Config().set("window_size", [event.size().width(), event.size().height()]) def _connect(self): if self.uiState == UIState.CONNECTED: self.cf.close_link() elif self.uiState == UIState.CONNECTING: self.cf.close_link() self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self.cf.open_link(self._selected_interface) def _scan(self): self.uiState = UIState.SCANNING self._update_ui_state() self.scanner.scanSignal.emit(self.address.value()) def _display_input_device_error(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _mux_selected(self, checked): """Called when a new mux is selected. The menu item contains a reference to the raw mux object as well as to the associated device sub-nodes""" if not checked: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(False) else: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(True) self.joystickReader.set_mux(mux=mux) # Go though the tree and select devices/mapping that was # selected before it was disabled. for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): dev_node.toggled.emit(True) self._update_input_device_footer() def _get_dev_status(self, device): msg = "{}".format(device.name) if device.supports_mapping: map_name = "No input mapping" if device.input_map: map_name = device.input_map_name msg += " ({})".format(map_name) return msg def _update_input_device_footer(self): """Update the footer in the bottom of the UI with status for the input device and its mapping""" msg = "" if len(self.joystickReader.available_devices()) > 0: mux = self.joystickReader._selected_mux msg = "Using {} mux with ".format(mux.name) for key in list(mux._devs.keys())[:-1]: if mux._devs[key]: msg += "{}, ".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A, " # Last item key = list(mux._devs.keys())[-1] if mux._devs[key]: msg += "{}".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A" else: msg = "No input device found" self._statusbar_label.setText(msg) def _inputdevice_selected(self, checked): """Called when a new input device has been selected from the menu. The data in the menu object is the associated map menu (directly under the item in the menu) and the raw device""" (map_menu, device, mux_menu) = self.sender().data() if not checked: if map_menu: map_menu.setEnabled(False) # Do not close the device, since we don't know exactly # how many devices the mux can have open. When selecting a # new mux the old one will take care of this. else: if map_menu: map_menu.setEnabled(True) (mux, sub_nodes) = mux_menu.data() for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): if device.id == dev_node.data()[1].id \ and dev_node is not self.sender(): dev_node.setChecked(False) role_in_mux = str(self.sender().parent().title()).strip() logger.info("Role of {} is {}".format(device.name, role_in_mux)) Config().set("input_device", str(device.name)) self._mapping_support = self.joystickReader.start_input( device.name, role_in_mux) self._update_input_device_footer() def _inputconfig_selected(self, checked): """Called when a new configuration has been selected from the menu. The data in the menu object is a referance to the device QAction in parent menu. This contains a referance to the raw device.""" if not checked: return selected_mapping = str(self.sender().text()) device = self.sender().data().data()[1] self.joystickReader.set_input_map(device.name, selected_mapping) self._update_input_device_footer() def device_discovery(self, devs): """Called when new devices have been added""" for menu in self._all_role_menus: role_menu = menu["rolemenu"] mux_menu = menu["muxmenu"] dev_group = QActionGroup(role_menu, exclusive=True) for d in devs: dev_node = QAction(d.name, role_menu, checkable=True, enabled=True) role_menu.addAction(dev_node) dev_group.addAction(dev_node) dev_node.toggled.connect(self._inputdevice_selected) map_node = None if d.supports_mapping: map_node = QMenu(" Input map", role_menu, enabled=False) map_group = QActionGroup(role_menu, exclusive=True) # Connect device node to map node for easy # enabling/disabling when selection changes and device # to easily enable it dev_node.setData((map_node, d)) for c in ConfigManager().get_list_of_configs(): node = QAction(c, map_node, checkable=True, enabled=True) node.toggled.connect(self._inputconfig_selected) map_node.addAction(node) # Connect all the map nodes back to the device # action node where we can access the raw device node.setData(dev_node) map_group.addAction(node) # If this device hasn't been found before, then # select the default mapping for it. if d not in self._available_devices: last_map = Config().get("device_config_mapping") if d.name in last_map and last_map[d.name] == c: node.setChecked(True) role_menu.addMenu(map_node) dev_node.setData((map_node, d, mux_menu)) # Update the list of what devices we found # to avoid selecting default mapping for all devices when # a new one is inserted self._available_devices = () for d in devs: self._available_devices += (d,) # Only enable MUX nodes if we have enough devies to cover # the roles for mux_node in self._all_mux_nodes: (mux, sub_nodes) = mux_node.data() if len(mux.supported_roles()) <= len(self._available_devices): mux_node.setEnabled(True) # TODO: Currently only supports selecting default mux if self._all_mux_nodes[0].isEnabled(): self._all_mux_nodes[0].setChecked(True) # If the previous length of the available devies was 0, then select # the default on. If that's not available then select the first # on in the list. # TODO: This will only work for the "Normal" mux so this will be # selected by default if Config().get("input_device") in [d.name for d in devs]: for dev_menu in self._all_role_menus[0]["rolemenu"].actions(): if dev_menu.text() == Config().get("input_device"): dev_menu.setChecked(True) else: # Select the first device in the first mux (will always be "Normal" # mux) self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True) logger.info("Select first device") self._update_input_device_footer() def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(cfclient.config_path))) def closeAppRequest(self): self.close() sys.exit(0)
class Hover: """Uses vision tracking and PID-control to hover at a given altitude 1. Run camera capture in it's own thread; update altitude 2. Run hover loop in it's own thread; output altitude """ def __init__(self): logger.info("{name} init()".format(name=self.__class__.__name__)) self.settings = shelve.open('/tmp/hover.settings') self.initControl() self.initCamera() self.initTracking() self.initHover() self.initCF() self.settings.close() def initControl(self): """Setup control-flow variables""" self.exit = False def initCamera(self): """Setup camera variables Will prompt the user for feedback. Capable of loading webcam or an ip-camera streaming JPEG. Uses shelve to remember last entered value """ # get settings lastCamUri = self.settings.get('camUri') lastCamRotate = self.settings.get('camRotate') # prompt for camUri, camRotate if lastCamUri: _input = raw_input("Specify camera: 'cam' for webcam, " + "an ip of network camera or <ENTER> for the " + "previous value of '{lastCamUri}':\n" .format(lastCamUri=lastCamUri)) if not _input: self.camUri = lastCamUri else: self.camUri = _input else: _input = raw_input("Specify camera: 'cam'/<ENTER> for webcam or " + "an ip of network camera:\n") if not _input: self.camUri = _input else: self.camUri = 'cam' logger.info("CamUri = '{camUri}'".format(camUri=self.camUri)) if lastCamRotate: _input = raw_input("Specify camera rotation or <ENTER> for the " + "previous value of '{lastCamRotate}':\n" .format(lastCamRotate=lastCamRotate)) if _input: self.camRotate = int(_input) else: self.camRotate = lastCamRotate else: _input = raw_input("Specify camera rotation or <ENTER> for " + "no rotation':\n") if _input: self.camRotate = int(_input) else: self.camRotate = 0 logger.info("CamRotate = '{camRotate}'"\ .format(camRotate=self.camRotate)) # save settings self.settings['camUri'] = self.camUri self.settings['camRotate'] = self.camRotate # setup camera if self.camUri == "cam": self.cam = Camera() elif '.' not in self.camUri: self.cam = JpegStreamCamera("http://192.168.1.{ip}:8080/video" .format(ip=self.camUri)) else: self.cam = JpegStreamCamera("http://{ip}:8080/video" .format(ip=self.camUri)) # additional values -- for now, just hard coded self.camRes = (800,600) logger.info("Camera resolution={res}".format(res=self.camRes)) def initTracking(self): """Setup tracking variables Will prompt the user for tracking variables. Uses shelve to remember last entered value """ # get last values lastTrackingColor = self.settings.get('trackingColor') # prompt for override if lastTrackingColor: _input = raw_input("Specify tracking color as (R,G,B)" + "or <ENTER> for previous value " + "{lastTrackingColor}:\n" .format(lastTrackingColor=lastTrackingColor)) if _input: self.trackingColor = make_tuple(_input) else: self.trackingColor = lastTrackingColor else: _input = raw_input("Specify tracking color as (R,G,B)" + "or <ENTER> for default of BLUE:\n") if _input: self.trackingColor = make_tuple(_input) else: self.trackingColor = Color.BLUE # save settings self.settings['trackingColor'] = self.trackingColor # additional values self.trackingBlobMin = 5 self.trackingBlobMax = 1000 self.x = -1 self.y = -1 self.target = (300,400) logger.info(("Tracking color={color}; min={min}; max={max}; " + "target={target}") .format(color=self.trackingColor, min=self.trackingBlobMin, max=self.trackingBlobMax, target=self.target)) self.trackingFrameQ = Queue() def initHover(self): self.hoverFrames = [] self.eSum = 0 def initCF(self): self.cfConnected = False self.cf = None self.cfUri = None logger.debug("Initializing Drivers; Debug=FALSE") InitDrivers(enable_debug_driver=False) logger.info("Scanning") available = Scan() logger.info("Found:") for cf in available: logger.debug(cf[0]) if len(available) > 0: self.cfUri = available[0][0] self.cf = Crazyflie() self.cf.connected.add_callback(self.cfOnConnected) self.cf.disconnected.add_callback(self.cfOnDisconnected) self.cf.connection_failed.add_callback(self.cfOnFailed) self.cf.connection_lost.add_callback(self.cfOnLost) logger.info("Crazyflie @{uri} Initialized".format(uri=self.cfUri)) else: logger.info("No Crazyflies found") def cfOnConnected(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) self.cfConnected = True def cfOnDisconnected(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) self.cfConnected = False def cfOnFailed(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) def cfOnLost(self, uri, msg): logger.info("{func} {uri} {msg}".format(func=inspect.stack()[0][3], uri=uri, msg=msg)) def visionLoop(self): while not self.exit: # acquire image img = self.cam.getImage() # exit if we've got nothing if img is None: break # adjust image if self.camRotate != 0: img.rotate(self.camrotate) ''' img = img.resize(self.camRes[0], self.camRes[1]) img = img.rotate90() ''' # blob search colorDiff = img - img.colorDistance(self.trackingColor) blobs = colorDiff.findBlobs(-1, self.trackingBlobMin, self.trackingBlobMax) ''' blobs = img.findBlobs((255,60,60), self.trackingBlobMin, self.trackingBlobMax) ''' # blob find if blobs is not None: self.x = blobs[-1].x self.y = blobs[-1].y # blob show if blobs is not None: # roi = region of interest roiLayer = DrawingLayer((img.width, img.height)) # draw all blobs for blob in blobs: blob.draw(layer=roiLayer) # draw a circle around the main blob roiLayer.circle((self.x,self.y), 50, Color.RED, 2) # apply roi to img img.addDrawingLayer(roiLayer) img = img.applyLayers() img.show() # fps now = datetime.utcnow() self.trackingFrameQ.put(now) if self.trackingFrameQ.qsize() < 30: fps = 0.0 else: fps = 30.0/(now - self.trackingFrameQ.get()).total_seconds() # logging logger.debug("{func} ({x},{y}) {fps:5.2f}" .format(func=inspect.stack()[0][3], x=self.x, y=self.y, fps=fps)) # loop has ened logger.debug("{func} stopped.".format(func=inspect.stack()[0][3])) def hoverLoop(self): while not self.exit: # hover throttled to camera frame rate sleep(1.0/30.0) # fps now = datetime.utcnow() self.hoverFrames.append(now) if len(self.hoverFrames) < 30: fps = 0.0 else: fps = 30.0/(now - self.hoverFrames[-30]).total_seconds() # pid variables kp = (3000.0/400.0) ki = 0.18 # determine immediate error e e = self.y - self.target[1] # determine accumulated errors eSum if len(self.hoverFrames) > 1: dt = (now - self.hoverFrames[-2]).total_seconds() else: dt = 0 self.eSum = self.eSum + e * dt # calculate thrust hoverThrust = 37000 thrust = hoverThrust + (kp * e) + (ki * self.eSum) # set the throttle self.setThrust(thrust) # logging logger.debug(("{func} e={e}, dt={dt:0.4f}, eSum={eSum:0.4f}, " + "thrust={thrust}, [{fps:5.2f}]") .format(func=inspect.stack()[0][3], e=e, dt=dt, eSum=self.eSum, thrust=thrust, fps=fps)) # loop has ened logger.debug("{func} stopped.".format(func=inspect.stack()[0][3])) def setThrust(self, thrust): """ sets thrust - but if control has exited, will kill thrust """ if self.exit: thrust = 0 if self.cf is not None: self.cf.commander.send_setpoint(0,0,0, thrust) def cfOpenLink(self): if self.cf is not None: self.cf.open_link(self.cfUri) logger.info("Linked opened to {uri}".format(uri=self.cfUri)) def start(self): logger.info("Starting VisionLoop") threading.Thread(target=self.visionLoop).start() logger.info("Starting HoverLoop") threading.Thread(target=self.hoverLoop).start() logger.info("Opening Crazyflie link") self.cfOpenLink() raw_input("Press enter to stop") self.stop() def stop(self): # kill our loops self.exit = True # explicitly kill thrust self.setThrust(0) # kill crazyflie if self.cf and self.cfConnected: self.cf.close_link()
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers(enable_debug_driver=GuiConfig() .get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback( self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) cfclient.ui.pluginhelper.inputDeviceReader.inputdevice.setBatteryData(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get( "device_config_mapping" )[self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText("Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText("Using [%s] with config [%s]" % ( self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
np.array([ self.odometry.roll, self.odometry.pitch, self.odometry.yaw ]) * np.pi / 180) f = np.dot( np.dot(R, np.transpose(-kz * e_r - kvz * e_v + mass * g * e_3)), e_3) roll_cmd = -k_theta * self.odometry.roll pitch_cmd = -k_theta * self.odometry.pitch yaw_cmd = -k_theta * self.odometry.yaw a = 2.13e-11 b = 1.032e-6 c = 5.485e-4 if f > 0: thrust = int(f / (0.057 * g) * 65535) if thrust >= 35535: thrust = 35534 else: thrust = 0 print(e_r, e_v, f, thrust) self.cf.commander.send_setpoint(roll_cmd, pitch_cmd, yaw_cmd, thrust) time.sleep(self.rate) self.cf.high_level_commander.land(0.0, 1.0) pid = PID_controller(cf) pid.goto([0, 0, 0.3]) cf.close_link()
class LoggingExample: """ Simple logging example class that logs the Stabilizer from a supplied link uri and disconnects after 5s. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie(rw_cache='./cache') # 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) self.pubLS = rospy.Publisher('cf_tof_ranges', LaserScan, queue_size=10) self.pubRup = rospy.Publisher('cf_tof_up', Range, queue_size=10) rospy.init_node('talker', anonymous=True) self.pubLS_msg = LaserScan() self.pubLS_msg.angle_min = 0.0 self.pubLS_msg.angle_max = 3.14159 / 2 * 3 self.pubLS_msg.angle_increment = 3.14159 / 2 self.pubLS_msg.range_min = 0.0 self.pubLS_msg.range_max = 100.0 self.pubLS_msg.header.frame_id = 'cf_base' self.range_msg_up = Range() self.range_msg_up.header.frame_id = 'cf_base' self.range_msg_up.min_range = 0.0 self.range_msg_up.max_range = 100.0 self.range_msg_up.field_of_view = 0.131 # 7.5 degrees 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 signal.signal(signal.SIGINT, self._signal_handler) 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) # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='RANGES', period_in_ms=10) self._lg_stab.add_variable('range.back', 'uint16_t') self._lg_stab.add_variable('range.left', 'uint16_t') self._lg_stab.add_variable('range.front', 'uint16_t') self._lg_stab.add_variable('range.right', 'uint16_t') self._lg_stab.add_variable('range.up', 'uint16_t') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we # would like to log are in the TOC. try: self._cf.log.add_config(self._lg_stab) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Stabilizer log config, bad configuration.') # except KeyboardInterrupt: # rethrow() # Start a timer to disconnect in 10s # t = Timer(5, self._cf.close_link) # t.start() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" # print('[%d][%s]: %s' % (timestamp, logconf.name, data)) #rangeStr = '[%d][%s]: %s' % (timestamp, logconf.name, data) #self.pub.publish(rangeStr) now = rospy.get_rostime() self.pubLS_msg.header.stamp.secs = now.secs self.pubLS_msg.header.stamp.nsecs = now.nsecs self.pubLS_msg.ranges = [ data['range.front'] * 0.001, data['range.left'] * 0.001, data['range.back'] * 0.001, data['range.right'] * 0.001 ] #self.pubLS.publish(self.pubLS_msg) self.range_msg_up.range = data['range.up'] * 0.001 self.range_msg_up.header.stamp.secs = now.secs self.range_msg_up.header.stamp.nsecs = now.nsecs #self.pubRup.publish(self.range_msg_up) 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 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)) 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 def _signal_handler(self, sig, frame): print('You pressed Ctrl+C!') self.is_connected = False self._cf.close_link() sys.exit(0)
class CrazyflieROS: Disconnected = 0 Connecting = 1 Connected = 2 """Wrapper between ROS and Crazyflie SDK""" def __init__(self, link_uri, tf_prefix, roll_trim, pitch_trim): self.link_uri = link_uri self.tf_prefix = tf_prefix self.roll_trim = roll_trim self.pitch_trim = pitch_trim self._cf = Crazyflie() 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._cmdVel = Twist() self._subCmdVel = rospy.Subscriber("cmd_vel", Twist, self._cmdVelChanged) self._pubImu = rospy.Publisher('imu', Imu, queue_size=10) self._pubTemp = rospy.Publisher('temperature', Temperature, queue_size=10) self._pubMag = rospy.Publisher('magnetic_field', MagneticField, queue_size=10) self._pubPressure = rospy.Publisher('pressure', Float32, queue_size=10) self._pubBattery = rospy.Publisher('battery', Float32, queue_size=10) self._pubTest = rospy.Publisher('test', Float32, queue_size=10) self._state = CrazyflieROS.Disconnected Thread(target=self._update).start() def _try_to_connect(self): rospy.loginfo("Connecting to %s" % self.link_uri) self._state = CrazyflieROS.Connecting self._cf.open_link(self.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.""" rospy.loginfo("Connected to %s" % link_uri) self._state = CrazyflieROS.Connected self._lg_imu = LogConfig(name="IMU", period_in_ms=10) self._lg_imu.add_variable("acc.x", "float") self._lg_imu.add_variable("acc.y", "float") self._lg_imu.add_variable("acc.z", "float") self._lg_imu.add_variable("gyro.x", "float") self._lg_imu.add_variable("gyro.y", "float") self._lg_imu.add_variable("gyro.z", "float") self._cf.log.add_config(self._lg_imu) if self._lg_imu.valid: # This callback will receive the data self._lg_imu.data_received_cb.add_callback(self._log_data_imu) # This callback will be called on errors self._lg_imu.error_cb.add_callback(self._log_error) # Start the logging self._lg_imu.start() else: rospy.logfatal( "Could not add logconfig since some variables are not in TOC") self._lg_log2 = LogConfig(name="LOG2", period_in_ms=100) self._lg_log2.add_variable("mag.x", "float") self._lg_log2.add_variable("mag.y", "float") self._lg_log2.add_variable("mag.z", "float") self._lg_log2.add_variable("baro.temp", "float") self._lg_log2.add_variable("baro.pressure", "float") self._lg_log2.add_variable("pm.vbat", "float") self._lg_log2.add_variable("test.tval", "float") self._cf.log.add_config(self._lg_log2) if self._lg_log2.valid: # This callback will receive the data self._lg_log2.data_received_cb.add_callback(self._log_data_log2) # This callback will be called on errors self._lg_log2.error_cb.add_callback(self._log_error) # Start the logging self._lg_log2.start() else: rospy.logfatal( "Could not add logconfig since some variables are not in TOC") p_toc = self._cf.param.toc.toc for group in p_toc.keys(): self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback) for name in p_toc[group].keys(): ros_param = "~{}/{}".format(group, name) cf_param = "{}.{}".format(group, name) if rospy.has_param(ros_param): self._cf.param.set_value(cfparam, rospy.get_param(ros_param)) else: self._cf.param.request_param_update(cf_param) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" rospy.logfatal("Connection to %s failed: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" rospy.logfatal("Connection to %s lost: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" rospy.logfatal("Disconnected from %s" % link_uri) self._state = CrazyflieROS.Disconnected def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" rospy.logfatal("Error when logging %s: %s" % (logconf.name, msg)) def _log_data_imu(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Imu() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" msg.orientation_covariance[0] = -1 # orientation not supported # measured in deg/s; need to convert to rad/s msg.angular_velocity.x = math.radians(data["gyro.x"]) msg.angular_velocity.y = math.radians(data["gyro.y"]) msg.angular_velocity.z = math.radians(data["gyro.z"]) # measured in mG; need to convert to m/s^2 msg.linear_acceleration.x = data["acc.x"] * 9.81 msg.linear_acceleration.y = data["acc.y"] * 9.81 msg.linear_acceleration.z = data["acc.z"] * 9.81 self._pubImu.publish(msg) #print "[%d][%s]: %s" % (timestamp, logconf.name, data) def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg) # Test msg.data = data["test.tval"] self._pubTest.publish(msg) def _param_callback(self, name, value): ros_param = "~{}".format(name.replace(".", "/")) rospy.set_param(ros_param, value) def _send_setpoint(self): roll = self._cmdVel.linear.y + self.roll_trim pitch = self._cmdVel.linear.x + self.pitch_trim yawrate = self._cmdVel.angular.z thrust = max(10000, int(self._cmdVel.linear.z)) #print(roll, pitch, yawrate, thrust) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) def _cmdVelChanged(self, data): self._cmdVel = data self._send_setpoint() def _update(self): while not rospy.is_shutdown(): if self._state == CrazyflieROS.Disconnected: self._try_to_connect() elif self._state == CrazyflieROS.Connected: # Crazyflie will shut down if we don't send any command for 500ms # Hence, make sure that we don't wait too long # However, if there is no connection anymore, we try to get the flie down if self._subCmdVel.get_num_connections() > 0: self._send_setpoint() else: self._cmdVel = Twist() self._send_setpoint() rospy.sleep(0.2) else: rospy.sleep(0.5) self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing rospy.sleep(0.1) self._cf.close_link()
class Sling: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self.thrust = 25000 self.pitch = 0 self.roll = 0 self.yawrate = 0 self.Run = False 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) # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) # Scan for Crazyflies and use the first one found #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! self.Run = True Thread(target=self._ramp_motors).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) 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) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" # print "Disconnected from %s" % link_uri def _ramp_motors(self): try: thrust_mult = 1 thrust_step = 300 count = 0 #Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) while self.Run: self._cf.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust) time.sleep(0.2) #print("sling:" + str(self.thrust)) """if thrust >= 35000: while count < 30: time.sleep(0.05) count += 1 self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) if count >= 30: thrust_mult = -1 thrust += thrust_mult*thrust_step""" self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing except: print("Unexpected error:", sys.exc_info()[0]) raise finally: time.sleep(0.1) self._cf.close_link()
class formationControl(threading.Thread): """ A controller thread, taking references and mearsurements from the UAV computing a control input""" def __init__(self, link_uri): threading.Thread.__init__(self) #self.link_uri = link_uri self.daemon = True self.timePrint = 0.0 self.timeplt = time.time() # Initialize the crazyflie object and add some callbacks 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.rate = 20 # (Hz) Rate of each sending control input self.statefb_rate = 50 # (ms) Time between two consecutive state feedback # Logged states -, attitude(p, r, y), rotation matrix self.position = [0.0, 0.0, 0.0] # [m] in the global frame of reference self.velocity = [0.0, 0.0, 0.0] # [m/s] in the global frame of reference self.attitude = [0.0, 0.0, 0.0] # [rad] attitude with inverted roll (r) self.rotMat = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] # Limits self.thrust_limit = (30000, 50000) self.roll_limit = (-30.0, 30.0) # [Degree] self.pitch_limit = (-30.0, 30.0) # [Degree] self.yaw_limit = (-200.0, 200.0) # [Degree] # Control setting self.isEnabled = True self.printDis = True # System parameter self.pi = 3.14159265359 def _run_controller(self): """ Main control loop # System parameters""" m = 0.032 # [kg] actual mass with some decks g = 9.799848 # [m/s^2] gravitational acceleration timeSampling = 1.0 / float(self.rate) # [s] sampling time number_Quad = 1 # [] number of crazyflie used delta = 2 # [] if quad knows reference -> 1, otherwise 0 # Control Parameters gv = 1.5 # for velocity gp = 0.7 # for position # Reference parameters v0 = [0.0, 0.0, 0.0] # Reference velocity fp = [0.0, 0.0, 0.0] # [m] the formation shape fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape coef_init = 1.0 # initialize altitude rot_mat = [ [0.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 0.0, 0.0], # shape in 3D space [0.0, 0.0, 0.0] ] # Set the current reference to the current positional estimate time.sleep(5) # Hover at z = 0.3 self._cf.commander.send_hover_setpoint(0, 0, 0, 0.3) time.sleep(2.0) x_0, y_0, z_0 = self.position yaw_d = self.attitude[2] print('Initial position reference: ({}, {}, {})'.format(x_0, y_0, z_0)) x_d, y_d, z_d = [x_0, y_0, z_0] # Unlock the controller # First, we need send a signal to enable sending attitude reference and thrust force self._cf.commander.send_setpoint(0, 0, 0, 0) # Begin while loop for sending the control signal timeBeginCtrl = time.time() while True: timeStart = time.time() # Change the reference velocity if time.time() > timeBeginCtrl + 0.2: v0 = [0.02, -0.02, 0.0] rot_mat = [ [1.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 1.0, 0.0], # shape in 3D space [0.0, 0.0, 1.0] ] coef_init = 0.0 #if time.time() > timeBeginCtrl + 10.0: # v0 = [0.0, 0.0, -0.1] # Landing if time.time() > timeBeginCtrl + 11.0: #self._cf.commander.send_setpoint(0, 0, 0, 0) self._cf.commander.send_position_setpoint(x=x, y=y, z=0.0, yaw=yaw) time.sleep(0.5) self._cf.close_link() print('Disconnect timeout') # Get the reference x_d = x_d + v0[0] * timeSampling y_d = y_d + v0[1] * timeSampling z_d = z_d + v0[2] * timeSampling # Get roll, pitch, yaw roll, pitch, yaw = self.attitude x, y, z = self.position vx, vy, vz = self.velocity # Get eta eta_px = x - (rot_mat[0][0] * fp[0] + rot_mat[0][1] * fp[1] + rot_mat[0][2] * fp[2]) - coef_init * x_0 eta_py = y - (rot_mat[1][0] * fp[0] + rot_mat[1][1] * fp[1] + rot_mat[1][2] * fp[2]) - coef_init * y_0 eta_pz = z - (rot_mat[2][0] * fp[0] + rot_mat[2][1] * fp[1] + rot_mat[2][2] * fp[2]) - coef_init * z_0 eta_vx = vx - fv[0] eta_vy = vy - fv[1] eta_vz = vz - fv[2] # Get u u_x = -gp * delta * (eta_px - x_d) - gv * delta * (eta_vx - v0[0]) u_y = -gp * delta * (eta_py - y_d) - gv * delta * (eta_vy - v0[1]) u_z = -2 * gp * delta * (eta_pz - z_d) - 2 * gv * delta * (eta_vz - v0[2]) u = [u_x, u_y, u_z] self.datalog_Pos.write( str(time.time()) + "," + str(self.position[0] - x_d) + "," + str(self.position[1] - y_d) + "," + str(self.position[2] - z_d) + "\n") if self.isEnabled: # Get thrust and attitude desired thrust_d, roll_d, pitch_d = self.output2nd(u, yaw, m, g) message = ( 'ref:({}, {}, {})\n'.format(x_d, y_d, z_d) + 'pos:({}, {}, {})\n'.format(x, y, z) + 'att:({}, {}, {})\n'.format(roll, pitch, yaw) + 'control:({}, {}, {})\n'.format(roll_d, pitch_d, thrust_d)) else: thrust_d, roll_d, pitch_d, yaw_d = (0.0, 0.0, 0.0, 0.0) self._cf.close_link() #print('Force disconnecting') # Send set point self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, thrust_d) #elf._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, 43000) self.print_at_period(2.0, message) self.loop_sleep(timeStart) # End def output2nd(self, u_out, yaw, m, g): """ This calculates the thrust force and attitude desired """ # Calculate tau tau1 = m * (cos(yaw) * u_out[0] + sin(yaw) * u_out[1]) tau2 = m * (-sin(yaw) * u_out[0] + cos(yaw) * u_out[1]) tau3 = m * (u_out[2] + g) # Calculate thrust and attitude desired thrust = sqrt(tau1 * tau1 + tau2 * tau2 + tau3 * tau3) roll_d = asin(-tau2 / thrust) pitch_d = atan2(tau1, tau3) # thrust in 16bit and angle to degree thrust_16bit_limit = self.thrust2cmd(thrust) roll_d = roll_d * 180 / self.pi pitch_d = pitch_d * 180 / self.pi # Saturation of roll and pitch desired roll_d_limit = self.saturation(roll_d, self.roll_limit) pitch_d_limit = self.saturation(pitch_d, self.pitch_limit) return [thrust_16bit_limit, roll_d_limit, pitch_d_limit] def thrust2cmd(self, thrust): """ This is able to transform thrust in Newton to command in integer 0-65000 We need to solve the second order polynominal to find cmd_thrust in integer corresponding to thrust in Newton thrust = 4 * (a * cmd_thrust ^ 2 + b * cmd_thrust + c)""" a = 2.130295e-11 b = 1.032633e-6 c = 5.48456e-4 delta = b * b - 4 * a * (c - thrust / 4.0) cmd_thrust = (-b + sqrt(delta)) / (2 * a) cmd_thrust = int(round(cmd_thrust)) cmd_thrust = self.saturation(cmd_thrust, self.thrust_limit) return cmd_thrust def saturation(self, value, limits): """ Saturate a given value within limit interval""" if value < limits[0]: value = limits[0] #print("limit low") elif value > limits[1]: value = limits[1] #print("limit up") return value def print_at_period(self, period, message): """ Prints the message at a given period""" if (time.time() - period) > self.timePrint: self.timePrint = time.time() print(message) def _connected(self, link_uri): """ This callback is called from the Crazyflie API when a Crazyflie has been connected adn TOCs have been downloaded """ print('connected to: %s' % link_uri) # Open text file for recording data self.datalog_Pos = open("log_data/Cf2log_Pos", "w") #self.datalog_Vel = open("log_data/Cf2log_Vel","w") #self.datalog_Att = open("log_data/Cf2log_Att","w") # Reset Kalman filter self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) print('Wait for Kalamn filter') # Feedback position estimated by KF logPos = LogConfig(name='Kalman Position', period_in_ms=self.statefb_rate) logPos.add_variable('kalman.stateX', 'float') logPos.add_variable('kalman.stateY', 'float') logPos.add_variable('kalman.stateZ', 'float') # Feedback velocity estimated by KF logVel = LogConfig(name='Kalman Velocity', period_in_ms=self.statefb_rate) logVel.add_variable('kalman.statePX', 'float') logVel.add_variable('kalman.statePY', 'float') logVel.add_variable('kalman.statePZ', 'float') # Feedback Quaternion attitude estimated by KF logAtt = LogConfig(name='Kalman Attitude', period_in_ms=self.statefb_rate) logAtt.add_variable('kalman.q0', 'float') logAtt.add_variable('kalman.q1', 'float') logAtt.add_variable('kalman.q2', 'float') logAtt.add_variable('kalman.q3', 'float') # Invoke logged states self._cf.log.add_config(logPos) self._cf.log.add_config(logVel) self._cf.log.add_config(logAtt) # Start invoking logged states if logPos.valid and logVel.valid: # Position logPos.data_received_cb.add_callback(self.log_pos_callback) logPos.error_cb.add_callback(logging_error) logPos.start() # Velocity logVel.data_received_cb.add_callback(self.log_vel_callback) logVel.error_cb.add_callback(logging_error) logVel.start() # Quadternion attitude logAtt.data_received_cb.add_callback(self.log_att_callback) logAtt.error_cb.add_callback(logging_error) logAtt.start() else: print( "One or more of the variables in the configuration was not found" + "in log TOC. No logging will be possible") # Start in a thread threading.Thread(target=self._run_controller).start() def log_pos_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.position = [ data['kalman.stateX'], data['kalman.stateY'], data['kalman.stateZ'] ] #self.datalog_Pos.write(str([time.time(), self.position]) + "\n") #print('Position x, y, z, time', self.position, time.time()) #print(self.rate) def log_att_callback(self, timestamp, data, Logconf): """ Callback for the logging the quadternion attitude of the UAV which is converted to roll, pitch, yaw in radians""" q = [ data['kalman.q0'], data['kalman.q1'], data['kalman.q2'], data['kalman.q3'] ] #Convert the quadternion to pitch roll and yaw yaw = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) pitch = asin(-2 * (q[1] * q[3] - q[0] * q[2])) roll = atan2(2 * (q[2] * q[3] + q[0] * q[1]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]) self.attitude = [roll, pitch, yaw] # Convert the quaternion to a rotation matrix R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] R[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3] R[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3] R[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2] R[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3] R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3] R[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1] R[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2] R[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1] R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3] self.rotMat = R #self.datalog_Att.write(str([time.time(), self.attitude]) + "\n") #print('Attitude r, p, y', self.attitude) def log_vel_callback(self, timestamp, data, Logconf): """ Callback for logging the velocity of the UAV defined w.r.t the body frame this subsequently rotated to the global frame""" PX, PY, PZ = [ data['kalman.statePX'], data['kalman.statePY'], data['kalman.statePZ'] ] R = self.rotMat self.velocity[0] = R[0][0] * PX + R[0][1] * PY + R[0][2] * PZ self.velocity[1] = R[1][0] * PX + R[1][1] * PY + R[1][2] * PZ self.velocity[2] = R[2][0] * PX + R[2][1] * PY + R[2][2] * PZ #self.datalog_Vel.write(str([time.time(), self.velocity]) + "\n") #print('Velocity rx, ry, rz', self.velocity) def _connection_failed(self, link_uri, msg): """ Callback when connection initial connection fails there is no Crazyflie at the specified address """ print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """ Callback when disconected after a connection has been made """ print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """ Callback when the Crazyflie is disconnected (called in all cases) """ if self.printDis: print('Disconnected from %s' % link_uri) self.printDis = False def loop_sleep(self, timeStart): """ Sleeps the control loop to make it run at a specified rate """ deltaTime = 1.0 / float(self.rate) - (time.time() - timeStart) if deltaTime > 0: time.sleep(deltaTime) def _close_connection(self, message): """ This is able to close link connecting Crazyflie from keyboard """ if message == "q": self.isEnabled = False # end def display(self): plt.scatter(time.time() - self.timeplt, self.position[2]) plt.pause(0.1)
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) ###################################################### ### By lxrocks ### 'Skinny Progress Bar' tweak for Yosemite ### Tweak progress bar - artistic I am not - so pick your own colors !!! ### Only apply to Yosemite ###################################################### import platform if platform.system() == 'Darwin': (Version,junk,machine) = platform.mac_ver() logger.info("This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars ") yosemite = (10,10,0) tVersion = tuple(map(int, (Version.split(".")))) if tVersion >= yosemite: logger.info( "Found Yosemite:") tcss = """ QProgressBar { border: 2px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: #05B8CC; } """ self.setStyleSheet(tcss) else: logger.info( "Pre-Yosemite") ###################################################### self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers(enable_debug_driver=Config() .get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect(self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self._connect) self.logConfigAction.triggered.connect(self._show_connect_dialog) self.connectButton.clicked.connect(self._connect) self.quickConnectButton.clicked.connect(self._quick_connect) self.menuItemQuickConnect.triggered.connect(self._quick_connect) self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_vbatt) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self._update_ui_state(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self._update_ui_state(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self._update_ui_state(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_mux, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m, self._menu_mux, checkable=True, enabled=True) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_mux.addAction(node) # TODO: Temporary self._input_dev_stack = [] self._menu_mux.actions()[0].setChecked(True) if Config().get("enable_input_muxing"): self._menu_mux.setEnabled(True) else: logger.info("Input device muxing disabled in config") self._mapping_support = True def _update_ui_state(self, newState, linkURI=""): self.uiState = newState if newState == UIState.DISCONNECTED: self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self._menu_cf2_config.setEnabled(False) self._menu_cf1_config.setEnabled(True) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if len(Config().get("link_uri")) > 0: self.quickConnectButton.setEnabled(True) if newState == UIState.CONNECTED: s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) # Find out if there's an I2C EEPROM, otherwise don't show the # dialog. if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0: self._menu_cf2_config.setEnabled(True) self._menu_cf1_config.setEnabled(False) if newState == UIState.CONNECTING: s = "Connecting to {} ...".format(linkURI) self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.available_devices() if (len(devs) > 0): self.device_discovery(devs) def _show_input_device_config_dialog(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked Config().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: {}".format(checked)) def _show_connect_dialog(self): self.logConfigDialogue.show() def _update_vbatt(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def _connected(self, linkURI): self._update_ui_state(UIState.CONNECTED, linkURI) Config().set("link_uri", str(linkURI)) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") try: self.cf.log.add_config(lg) lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() except KeyError as e: logger.warning(str(e)) def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [{}]: {}".format(log_conf.name, msg)) def _connection_lost(self, linkURI, msg): if not self._auto_reconnect_enabled: if self.isActiveWindow(): warningCaption = "Communication failure" error = "Connection lost to {}: {}".format(linkURI, msg) QMessageBox.critical(self, warningCaption, error) self._update_ui_state(UIState.DISCONNECTED, linkURI) else: self._quick_connect() def _connection_failed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on {}: {}".format(linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self._update_ui_state(UIState.DISCONNECTED, linkURI) else: self._quick_connect() def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def _connect(self): if self.uiState == UIState.CONNECTED: self.cf.close_link() elif self.uiState == UIState.CONNECTING: self.cf.close_link() self._update_ui_state(UIState.DISCONNECTED) else: self.connectDialogue.show() def _display_input_device_error(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self._active_config = str(config) self._active_device = str(device) Config().set("input_device", str(self._active_device)) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [{}] - " "No input config selected".format (self._active_device)) else: self._statusbar_label.setText("Using [{}] with config [{}]".format (self._active_device, self._active_config)) def _mux_selected(self, checked): if not checked: return selected_mux_name = str(self.sender().text()) self.joystickReader.set_mux(name=selected_mux_name) logger.debug("Selected mux supports {} devices".format(self.joystickReader.get_mux_supported_dev_count())) self._adjust_nbr_of_selected_devices() def _get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in device_config_mapping.keys(): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def _update_input_device_footer(self, device_name=None, mapping_name=None): """Update the footer in the bottom of the UI with status for the input device and its mapping""" if not device_name and not mapping_name: self._statusbar_label.setText("No input device selected") elif self._mapping_support and not mapping_name: self._statusbar_label.setText("Using [{}] - " "No input config selected".format( device_name)) elif not self._mapping_support: self._statusbar_label.setText("Using [{}]".format(device_name)) else: self._statusbar_label.setText("Using [{}] with config [{}]".format( device_name, mapping_name)) def _adjust_nbr_of_selected_devices(self): nbr_of_selected = len(self._input_dev_stack) nbr_of_supported = self.joystickReader.get_mux_supported_dev_count() while len(self._input_dev_stack) > nbr_of_supported: to_close = self._input_dev_stack.pop(0) # Close and de-select it in the UI self.joystickReader.stop_input(to_close) for c in self._menu_devices.actions(): if c.text() == to_close: c.setChecked(False) def _inputdevice_selected(self, checked): """Called when a new input device has been selected from the menu""" if not checked: return self._input_dev_stack.append(self.sender().text()) selected_device_name = str(self.sender().text()) self._active_device = selected_device_name # Save the device as "last used device" Config().set("input_device", str(selected_device_name)) # Read preferred config used for this controller from config, # if found then select this config in the menu self._mapping_support = self.joystickReader.start_input(selected_device_name) self._adjust_nbr_of_selected_devices() if self.joystickReader.get_mux_supported_dev_count() == 1: preferred_config = self.joystickReader.get_saved_device_mapping(selected_device_name) if preferred_config: for c in self._menu_mappings.actions(): if c.text() == preferred_config: c.setChecked(True) def _inputconfig_selected(self, checked): """Called when a new configuration has been selected from the menu""" if not checked: return selected_mapping = str(self.sender().text()) self.joystickReader.set_input_map(self._active_device, selected_mapping) self._update_input_device_footer(self._active_device, selected_mapping) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=False) for d in devs: node = QAction(d.name, self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if d.name == Config().get("input_device"): self._active_device = d.name if len(self._active_device) == 0: self._active_device = str(self._menu_devices.actions()[0].text()) device_config_mapping = Config().get("device_config_mapping") if device_config_mapping: if self._active_device in device_config_mapping.keys(): self._current_input_config = device_config_mapping[ self._active_device] else: self._current_input_config = self._menu_mappings.actions()[0]\ .text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_devices.actions(): if c.text() == self._active_device: c.setChecked(True) for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._current_input_config: c.setChecked(True) def _quick_connect(self): try: self.cf.open_link(Config().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class crazy_command: """Example that connects to a Crazyflie and send command to the motors and the disconnects""" global commandsToGo copter_index = 0 def __init__(self, link_uri, copter_index): """ Initialize and run the example with the specified link_uri """ self.copter_index = copter_index 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.is_connected = True print('Connecting to %s' % link_uri) self._cf.commander.send_setpoint(0, 0, 0, 0) 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._send_commands_thread).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.is_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)) 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 def _command_motors(self): # Unlock startup thrust protection roll = commandsToGo[0][0] pitch = commandsToGo[0][1] yawrate = commandsToGo[0][3] thrust = commandsToGo[0][2] # print(thrust) # self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing #time.sleep(0.1) #self._cf.close_link() def _send_commands(self, commands): # roll = commandsToGo[0][0] # pitch = commandsToGo[0][1] # yawrate = commandsToGo[0][3] # thrust = commandsToGo[0][2] self._cf.commander.send_setpoint(commands[0], commands[1], commands[3], commands[2]) # print(commands[2]) def _send_commands_thread(self): # roll = commandsToGo[0][0] # pitch = commandsToGo[0][1] # yawrate = commandsToGo[0][3] # thrust = commandsToGo[0][2] while (self.is_connected): self._cf.commander.send_setpoint( commandsToGo[self.copter_index][0], commandsToGo[self.copter_index][1], commandsToGo[self.copter_index][3], commandsToGo[self.copter_index][2]) time.sleep(0.005) # time.sleep(0.01) # print(commands[2]) def _close_it(self): self._cf.close_link() self.is_connected = False
class Main: def __init__(self): self.roll = 0.0 self.pitch = 0.0 self.yawrate = 0 self.thrust = 0 self._stop = 0; self.series_index = -1 self.series_data = [] self.series_thrust = [0, 10001, 20001, 30001, 40001, 50001, 60000] self.discard_data = True self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): print "Should be connected now...\n" lg = LogConfig("Magnetometer", 100) lg.addVariable(LogVariable("mag.x", "int16_t")) lg.addVariable(LogVariable("mag.y", "int16_t")) lg.addVariable(LogVariable("mag.z", "int16_t")) log = self.crazyflie.log.create_log_packet(lg) if log is not None: log.dataReceived.add_callback(self.magData) log.start() else: print "Magnetometer not found in log TOC" # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() Thread(target=self.input_loop).start() def stop(self): # Exit command received # Set thrust to zero to make sure the motors turn off NOW self.thrust = 0 # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1; self.process_data() # Exit the main loop print "Exiting main loop in 1 second" time.sleep(1) self.crazyflie.close_link() # This errors out for some reason. Bad libusb? sys.exit(0) def process_data(self): centers = [] for series in self.series_data: xs = [] ys = [] zs = [] for x, y, z in series: # correct hard and soft iron errors x = x - magn_ellipsoid_center[0] y = y - magn_ellipsoid_center[1] z = z - magn_ellipsoid_center[2] prod = np.dot(magn_ellipsoid_transform, [[x], [y], [z]]) x = prod[0][0] y = prod[1][0] z = prod[2][0] # store corrected values xs.append(x) ys.append(y) zs.append(z) center_x, center_y = fit_ellipse_center(xs, ys) center_z = np.median(np.array(zs)) centers.append((center_x, center_y, center_z)) self.curve_fit(centers) #self.show_plot() def show_plot(self): from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.set_xlabel('X Label') ax.set_ylabel('Y Label') ax.set_zlabel('Z Label') for series in self.series_data: xs = []; ys = []; zs = [] for x, y, z in series: xs.append(x) ys.append(y) zs.append(z) ax.scatter(np.array(xs), np.array(ys), np.array(zs)) plt.show() def curve_fit(self, centers): nn = np.array([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) xs = [] ys = [] zs = [] x0, y0, z0 = centers[0] for i in range(0, 7): x, y, z = centers[i] xs.append(x0 - x) ys.append(y0 - y) zs.append(z0 - z) print 'result' print 'qx =', list(np.polyfit(nn, xs, 3)) print 'qy =', list(np.polyfit(nn, ys, 3)) print 'qz =', list(np.polyfit(nn, zs, 3)) def input_loop(self): print "Beginning input loop:" while True: command = raw_input("Press Enter for next iteration (e or q will quit):") if (command=="exit") or (command=="e") or (command=="quit") or (command=="q"): self.stop() elif command == '': if self.series_index < 6: self.discard_data = True time.sleep(0.5) # do stuff self.series_index = self.series_index + 1 print 'Running next round, press rotate CF and hit enter when finished. Iteration', self.series_index self.series_data.append([]) self.thrust = self.series_thrust[self.series_index] time.sleep(0.5) self.discard_data = False else: print 'Finished calibration' self.stop() def pulse_command(self): while 1: self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust) time.sleep(0.1) # Exit if the parent told us to if self._stop==1: return def magData(self, data): x, y, z = data['mag.x'], data['mag.y'], data['mag.z'] if not self.discard_data: self.series_data[self.series_index].append((x, y, z))
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri) def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 32000 pitch = 0 roll = 0 yawrate = 0 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) print("Constant thrust") self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(1.2) print("Turning on altitude hold") self._cf.param.set_value("flightmode.althold", "True") self._cf.commander.send_setpoint(0, 0, 0, 32767) time.sleep(3) print("Closing link") self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self._cf.close_link()
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri) def take_off(self, roll, pitch, yawrate, thrust): x = 0 while x <= 4: #take off loop x = x+1 self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000) time.sleep(0.1) return def hover(self, roll, pitch, yawrate, thrust): x = 0 while x <= 10: #hover Loop x = x+1 self._cf.commander.send_setpoint(roll, pitch, yawrate, 32000) time.sleep(0.1) self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000) time.sleep(0.1) return def next_spot(self, roll, pitch, yawrate, thrust): x = 0 while x <= 3: #forward loop x = x+1 self._cf.commander.send_setpoint(roll, pitch , yawrate, 35000) time.sleep(0.1) #stop the quad from moving forward pitch = -2 self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000) time.sleep(0.1) pitch = 3 return def land(self, roll, pitch, yawrate, thrust): x = 0 thrust1 = 33000 while x <= 13: #landing loop x = x+1 self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1) time.sleep(0.1) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1) time.sleep(0.1) thrust1 = thrust1 - 100 return def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 35000 pitch = 3 roll = 1 yawrate = 0 y = 0 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self.take_off(roll, pitch, yawrate, thrust) while y == 0: self.hover(roll, pitch, yawrate, thrust) pitch = 10 self.next_spot(roll, pitch, yawrate, thrust) pitch = 3 y = 1 self.hover(roll, pitch, yawrate, thrust) thrust = 33000 self.land(roll, pitch, yawrate, thrust) self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self._cf.close_link()
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) print("Connecting to %s" % link_uri) def _connected(self, link_uri): print("connected to %s" % link_uri) self._roll_list = [0.0,0.0,0.0] self._pitch_list = [0.0,0.0,0.0] self._yaw_list = [0.0,0.0,0.0] self.roll_i = 0 self.pitch_i = 0 self.roll = 0 self.pitch = 0 self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10) self._lg_stab.add_variable("stabilizer.roll", "float") self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") try: self._cf.log.add_config(self._lg_stab) self._lg_stab.data_received_cb.add_callback(self._stab_log_data) self._lg_stab.error_cb.add_callback(self._stab_log_error) self._lg_stab.start() except KeyError as e: print("Could not start log configuration," "{} not found in TOC".format(str(e))) except AttributeError: print("Could not add Stabilizer log config, bad configuration.") self._power_on = True self._suspend = False motor_thrd = Thread(target=self._ramp_motors) cmd_thrd = Thread(target=self._controlcenter) motor_thrd.start() cmd_thrd.start() def _controlcenter(self): time.sleep(2) while True: cmd=raw_input("cmd->") if cmd == "k" or cmd == "kill": self._power_on=False print("coptor-> stoped by the command \'kill\'\n") if cmd == "s" or cmd == "suspend": self._suspend = True print("coptor-> suspendind\n") if cmd == "l" or cmd == "land": self._suspend = False print("coptor-> landing\n") def _ramp_motors(self): try: time.sleep(2) inithrust = 32000 maxthrust = 38000 minthrust = 27000 duration = 5 f = open('./log','w') self._ramp_takeoff(inithrust, maxthrust, f) self._ramp_suspend(maxthrust, duration, f) self._ramp_landing(maxthrust, minthrust, f) print("mission complete\n") sys.stdout.flush() time.sleep(0.5) except AttributeError as attr_error: print("\t!! we confronted with an AttributeError: %s. link closed." % str(attr_error)) except TypeError as type_error: print("\t!! we confronted with an TypeError: %s. link closed." % str(type_error)) finally: self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.5) self._cf.close_link() f.close() def _ramp_takeoff(self, inithrust, maxthrust, logfile): thrust = inithrust thrust_step = 900 f = logfile self._cf.commander.send_setpoint(0,0,0,0) while thrust <= maxthrust and self._power_on: (roll,pitch,yawrate) = self._pid(self._roll_list,self._pitch_list) thrust += thrust_step self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n') f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n') f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n') f.write('\n') time.sleep(0.1) def _ramp_suspend(self, suspendthrust, seconds, logfile): thrust = suspendthrust f = logfile i=0 while (i<(seconds*10) or self._suspend) and self._power_on: roll, pitch, yawrate = self._pid(self._roll_list,self._pitch_list) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n') f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n') f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n') f.write('roll_send:'+str(roll)+'\t\t\tpitch_send'+str(pitch)) f.write('\n\n') i +=1 time.sleep(0.1) def _ramp_landing(self, inithrust, minthrust, logfile): thrust = inithrust thrust_step = 250 f = logfile while thrust >= minthrust and self._power_on: roll,pitch,yawrate = self._pid(self._roll_list,self._pitch_list) thrust -= thrust_step self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n') f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n') f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n') f.write('\n') time.sleep(0.1) def _pid(self, roll_list = None, pitch_list = None, yawrate_list = None): # position method: roll_bias = 0 pitch_bias = 0 roll_kp=1.75 # neg roll_ki=0 roll_kd=0.15 # pos pitch_kp=1.75 # neg pitch_ki=0 pitch_kd=0.1 # pos roll_now = roll_list[0] pitch_now = pitch_list[0] roll_p = roll_now self.roll_i += roll_now # accumulate roll_d = roll_now-roll_list[1] pitch_p = pitch_now self.pitch_i += pitch_now # accumulate pitch_d = pitch_now - pitch_list[1] roll = roll_kp * roll_p + roll_ki * self.roll_i + roll_kd * roll_d pitch = pitch_kp * pitch_p + pitch_ki * self.pitch_i + pitch_kd * pitch_d yawrate = 0 self._roll_list[2] = self._roll_list[1] self._roll_list[1] = roll_now self._pitch_list[2] = self._pitch_list[1] self._pitch_list[1] = pitch_now self._yaw_list[2] = self._yaw_list[1] self._yaw_list[1] = self._yaw_list[0] return (roll + roll_bias, pitch + pitch_bias, yawrate) def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print("Error when logging %s: %s" % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" self._roll_list[0] = data['stabilizer.roll'] self._pitch_list[0] = data['stabilizer.pitch'] self._yaw_list[0] = data['stabilizer.yaw'] 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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri)
class formationControl(threading.Thread): """ A controller thread, taking references and mearsurements from the UAV computing a control input""" def __init__(self, link_uri): threading.Thread.__init__(self) #self.link_uri = link_uri self.daemon = True self.timePrint = 0.0 self.timeplt = time.time() # Initialize the crazyflie object and add some callbacks 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.rate = 20 # (Hz) Rate of each sending control input self.statefb_rate = 50 # (ms) Time between two consecutive state feedback # Logged states -, attitude(p, r, y), rotation matrix self.position = [0.0, 0.0, 0.0] # [m] in the global frame of reference self.velocity = [0.0, 0.0, 0.0] # [m/s] in the global frame of reference self.attitude = [0.0, 0.0, 0.0] # [rad] attitude with inverted roll (r) self.rotMat = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] self.Mtr = [0.0, 0.0, 0.0, 0.0, 0.0] # Multiranger # Limits self.thrust_limit = (30000,50000) self.roll_limit = (-30.0, 30.0) # [Degree] self.pitch_limit = (-30.0, 30.0) # [Degree] self.yaw_limit = (-200.0, 200.0) # [Degree] # Control setting self.isEnabled = True self.printDis = True # System parameter self.pi = 3.14159265359 def _run_controller(self): """ Main control loop # System parameters""" m = 0.034 # [kg] actual mass with some decks g = 9.799848 # [m/s^2] gravitational acceleration timeSampling = 1.0/float(self.rate) # [s] sampling time number_Quad = 1 # [] number of crazyflie used delta = 2 # [] if quad knows reference -> 1, otherwise 0 # Control Parameters gv = 1.2 # for velocity gp = 0.5 # for position # Reference parameters v0 = [0.0, 0.0, 0.0] # Reference velocity r_amp = 0.5 r_omg = 0.5 r_k = 0.0 fp_ref = [0.0, 0.0, 0.0] # [m] the formation shape fp = [0.0, 0.0, 0.0] # [m] the formation shape fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape coef_init = 1.0 # initialize altitude rot_mat = [[0.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 0.0, 0.0], # shape in 3D space [0.0, 0.0, 0.0]] col_thrs = 0.3 safe_thrs = 0.9 col_muy = 2.0 col_lda = 0.0 Mtr_pre = self.Mtr Mtr_dot = [0.0, 0.0, 0.0, 0.0] # z compensation error_z = 0.0 error_z_pre = 0.0 z_ctrl_cp = 0.0 # Set the current reference to the current positional estimate time.sleep(5) # Unlock the controller # First, we need send a signal to enable sending attitude reference and thrust force self._cf.commander.send_setpoint(0, 0, 0, 0) # Hover at z = 0.3 durin 2 seconds x_0, y_0, z_0 = self.position yaw_0 = self.attitude[2] timeStart = time.time() while True: self._cf.commander.send_position_setpoint(x=x_0,y=y_0,z=0.4,yaw=yaw_0) time.sleep(0.2) if time.time() - timeStart > 2.0: break # Take a local coordinate x_0, y_0, z_0 = self.position yaw_d = self.attitude[2] print('Initial position reference: ({}, {}, {})'.format(x_0, y_0, z_0)) x_d, y_d, z_d = [x_0, y_0, z_0] # Begin while loop for sending the control signal timeBeginCtrl = time.time() while True: timeStart = time.time() # Change the reference velocity if time.time() > timeBeginCtrl + 0.5: #v0 = [0.0, -0.0, 0.0] v0[0] = r_omg * r_amp * cos(r_omg * r_k * timeSampling) v0[1] = - r_omg * r_amp * sin(r_omg * r_k * timeSampling) rot_mat = [[1.0, 0.0, 0.0], # Rotation matrix for formation [0.0, 1.0, 0.0], # shape in 3D space [0.0, 0.0, 1.0]] coef_init = 0.0 r_k = r_k + 1.0 if time.time() > timeBeginCtrl + 60.0: self._cf.commander.send_setpoint(0, 0, 0, 39000) self._cf.close_link() print('Disconnect timeout') # Get the reference x_d = x_d + v0[0] * timeSampling y_d = y_d + v0[1] * timeSampling z_d = z_d + v0[2] * timeSampling # Get fp using smooth step function fp[0] = fp_ref[0] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) fp[1] = fp_ref[1] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) fp[2] = fp_ref[2] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) fv[0] = fp_ref[0] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) fv[1] = fp_ref[1] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) fv[2] = fp_ref[2] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0) # Get roll, pitch, yaw roll, pitch, yaw = self.attitude x, y , z = self.position vx, vy, vz = self.velocity # Get eta eta_px = x - (rot_mat[0][0] * fp[0] + rot_mat[0][1] * fp[1] + rot_mat[0][2] * fp[2]) - coef_init * x_0 eta_py = y - (rot_mat[1][0] * fp[0] + rot_mat[1][1] * fp[1] + rot_mat[1][2] * fp[2]) - coef_init * y_0 eta_pz = z - (rot_mat[2][0] * fp[0] + rot_mat[2][1] * fp[1] + rot_mat[2][2] * fp[2]) - coef_init * z_0 eta_vx = vx - fv[0] eta_vy = vy - fv[1] eta_vz = vz - fv[2] error_z = z_d - eta_pz z_ctrl_cp = z_ctrl_cp + (error_z + error_z_pre) / 2.0 * timeSampling error_z_pre = error_z # Get u tracking u_x_tr = - gp * delta * (eta_px - x_d) u_y_tr = - gp * delta * (eta_py - y_d) u_z_tr = - gp * delta * (eta_pz - z_d) # Multi range dot Mtr_dot[0] = (self.Mtr[0] - Mtr_pre[0]) / timeSampling Mtr_dot[1] = (self.Mtr[1] - Mtr_pre[1]) / timeSampling Mtr_dot[2] = (self.Mtr[2] - Mtr_pre[2]) / timeSampling Mtr_dot[3] = (self.Mtr[3] - Mtr_pre[3]) / timeSampling # Collision avoidance u_x_c = - self.bump_fcn_dot(self.Mtr[0],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[0] \ + self.bump_fcn_dot(self.Mtr[1],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[1] u_y_c = - self.bump_fcn_dot(self.Mtr[2],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[2] \ + self.bump_fcn_dot(self.Mtr[3],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[3] u_z_c = self.bump_fcn_dot(self.Mtr[0],col_thrs,safe_thrs,col_muy,col_lda) \ * self.bump_fcn_dot(self.Mtr[1],col_thrs,safe_thrs,col_muy,col_lda) # Storage multi range information Mtr_pre = self.Mtr # Get u (tracking and collision avoidance) u_x = self.step_fcn(self.Mtr[0],col_thrs,safe_thrs) \ * self.step_fcn(self.Mtr[1],col_thrs,safe_thrs) \ * u_x_tr + u_x_c u_y = self.step_fcn(self.Mtr[2],col_thrs,safe_thrs) \ * self.step_fcn(self.Mtr[3],col_thrs,safe_thrs) \ * u_y_tr + u_y_c #+ u_x_c u_z = (self.step_fcn(self.Mtr[0],col_thrs,safe_thrs) \ + self.step_fcn(self.Mtr[1],col_thrs,safe_thrs))\ * u_z_tr + u_z_c #u_x = u_x_tr #u_y = u_y_tr u_z = u_z_tr u = [u_x, u_y, u_z] #print(u_z_c) x_d_c = u_x_c + x_d y_d_c = u_y_c + y_d # Storage data self.datalog_Pos.write(str(time.time()) + "," + str(self.position[0]) + "," + str(self.position[1]) + "," + str(self.position[2]) + "\n") # Check whether control is enable or not if self.isEnabled: # Get thrust and attitude desired thrust_d, roll_d, pitch_d = self.output2nd(u, yaw, m, g) message = ('ref:({}, {}, {})\n'.format(x_d, y_d, z_d) + 'pos:({}, {}, {})\n'.format(x, y, z) + 'att:({}, {}, {})\n'.format(roll, pitch, yaw) + 'control:({}, {}, {})\n'.format(roll_d, pitch_d, thrust_d)) else: thrust_d, roll_d, pitch_d, yaw_d = (0.0, 0.0, 0.0, 0.0) self._cf.close_link() print('Force disconnecting') # Send set point #self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, thrust_d) #self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, 43000) #self._cf.commander.send_position_setpoint(x_d_c,y_d_c,0.4,yaw_d) self._cf.commander.send_velocity_world_setpoint(u_x, u_y, u_z_tr, 0.0) self.print_at_period(2.0, message) self.loop_sleep(timeStart) # End def bump_fcn(self, mr, threshold, muy): """ This is bump function""" if mr > threshold: mr_bump = 0 else: mr_bump = (threshold - mr) * (threshold - mr) mr_bump = mr_bump/(mr + threshold * threshold * 1.0/muy) return mr_bump def bump_fcn_dot(self, mr, col_thrs, safe_thrs, muy, lda): """ This is the derivative of the bump function""" if mr > col_thrs: mr_bump_dot = lda * self.step_dot_fcn(mr, col_thrs, safe_thrs) else: mr_bump_dot = float(mr) + 2.0 * col_thrs * col_thrs / muy + col_thrs mr_bump_dot = - mr_bump_dot * (col_thrs - float(mr)) mr_bump_dot = mr_bump_dot / ((float(mr) + col_thrs * col_thrs / muy)*(float(mr) + col_thrs * col_thrs / muy)) return mr_bump_dot def step_fcn(self, mr, low, up): """ This is smooth step function """ if mr < low: step_out = 0 elif mr > low and mr < up: step_out = (float(mr) - low)/(up - low) step_out = step_out * step_out else: step_out = 1 return step_out def step_dot_fcn(self, mr, low, up): """ This is smooth step function """ if mr <= low or mr >= up: step_out = 0 else: step_out = 2*(float(mr) - low)/(up - low) step_out = step_out / (up - low) return step_out def output2nd(self, u_out, yaw, m, g): """ This calculates the thrust force and attitude desired """ # Calculate tau tau1 = m * (cos(yaw) * u_out[0] + sin(yaw) * u_out[1]) tau2 = m * (-sin(yaw) * u_out[0] + cos(yaw) * u_out[1]) tau3 = m * (u_out[2] + g) # Calculate thrust and attitude desired thrust = sqrt(tau1 * tau1 + tau2 * tau2 + tau3 * tau3) roll_d = asin(-tau2/thrust) pitch_d = atan2(tau1,tau3) # thrust in 16bit and angle to degree thrust_16bit_limit = self.thrust2cmd(thrust) roll_d = roll_d * 180.0 / self.pi pitch_d = pitch_d * 180.0 / self.pi # Saturation of roll and pitch desired roll_d_limit = self.saturation(roll_d, self.roll_limit) pitch_d_limit = self.saturation(pitch_d, self.pitch_limit) return [thrust_16bit_limit, roll_d_limit, pitch_d_limit] def thrust2cmd(self, thrust): """ This is able to transform thrust in Newton to command in integer 0-65000 We need to solve the second order polynominal to find cmd_thrust in integer corresponding to thrust in Newton thrust = 4 * (a * cmd_thrust ^ 2 + b * cmd_thrust + c)""" a = 2.130295e-11 #a = 2.230295e-11 b = 1.032633e-6 c = 5.48456e-4 delta = b * b - 4 * a * (c - thrust/4.0) cmd_thrust = (-b + sqrt(delta))/(2 * a) cmd_thrust = int(round(cmd_thrust)) cmd_thrust = self.saturation(cmd_thrust, self.thrust_limit) return cmd_thrust def saturation(self, value, limits): """ Saturate a given value within limit interval""" if value < limits[0]: value = limits[0] #print("limit low") elif value > limits[1]: value = limits[1] #print("limit up") return value def print_at_period(self, period, message): """ Prints the message at a given period""" if (time.time() - period) > self.timePrint: self.timePrint = time.time() print(message) def _connected(self, link_uri): """ This callback is called from the Crazyflie API when a Crazyflie has been connected adn TOCs have been downloaded """ print('connected to: %s' %link_uri) # Open text file for recording data self.datalog_Pos = open("log_data/Cf2log_Pos","w") #self.datalog_Vel = open("log_data/Cf2log_Vel","w") #self.datalog_Att = open("log_data/Cf2log_Att","w") self.datalog_Att = open("log_data/Cf2log_Mtr","w") # Reset Kalman filter self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) print('Wait for Kalamn filter') # Feedback position estimated by KF logPos = LogConfig(name='Kalman Position', period_in_ms=self.statefb_rate ) logPos.add_variable('kalman.stateX','float') logPos.add_variable('kalman.stateY','float') logPos.add_variable('kalman.stateZ','float') # Feedback velocity estimated by KF logVel = LogConfig(name='Kalman Velocity', period_in_ms=self.statefb_rate ) logVel.add_variable('kalman.statePX','float') logVel.add_variable('kalman.statePY','float') logVel.add_variable('kalman.statePZ','float') # Feedback Quaternion attitude estimated by KF logAtt = LogConfig(name='Kalman Attitude', period_in_ms=self.statefb_rate ) logAtt.add_variable('kalman.q0','float') logAtt.add_variable('kalman.q1','float') logAtt.add_variable('kalman.q2','float') logAtt.add_variable('kalman.q3','float') # Feedback Multi ranger logMultiRa = LogConfig(name='Range', period_in_ms=self.statefb_rate) logMultiRa.add_variable('range.front','uint16_t') logMultiRa.add_variable('range.back','uint16_t') logMultiRa.add_variable('range.left','uint16_t') logMultiRa.add_variable('range.right','uint16_t') logMultiRa.add_variable('range.up','uint16_t') # Invoke logged states self._cf.log.add_config(logPos) self._cf.log.add_config(logVel) self._cf.log.add_config(logAtt) self._cf.log.add_config(logMultiRa) # Start invoking logged states if logPos.valid and logVel.valid: # Position logPos.data_received_cb.add_callback(self.log_pos_callback) logPos.error_cb.add_callback(logging_error) logPos.start() # Velocity logVel.data_received_cb.add_callback(self.log_vel_callback) logVel.error_cb.add_callback(logging_error) logVel.start() # Quadternion attitude logAtt.data_received_cb.add_callback(self.log_att_callback) logAtt.error_cb.add_callback(logging_error) logAtt.start() # Multi-Ranger logMultiRa.data_received_cb.add_callback(self.log_mtr_callback) logMultiRa.error_cb.add_callback(logging_error) logMultiRa.start() else: print("One or more of the variables in the configuration was not found"+ "in log TOC. No logging will be possible") # Start in a thread threading.Thread(target=self._run_controller).start() def log_pos_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.position = [ data['kalman.stateX'], data['kalman.stateY'], data['kalman.stateZ'] ] #self.datalog_Pos.write(str([time.time(), self.position]) + "\n") #print('Position x, y, z, time', self.position, time.time()) #print(self.rate) def log_att_callback(self, timestamp, data, Logconf): """ Callback for the logging the quadternion attitude of the UAV which is converted to roll, pitch, yaw in radians""" q = [ data['kalman.q0'], data['kalman.q1'], data['kalman.q2'], data['kalman.q3'] ] #Convert the quadternion to pitch roll and yaw yaw = atan2(2*(q[1]*q[2]+q[0]*q[3]) , q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) pitch = asin(-2*(q[1]*q[3] - q[0]*q[2])) roll = atan2(2*(q[2]*q[3]+q[0]*q[1]) , q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) self.attitude = [roll, pitch, yaw] # Convert the quaternion to a rotation matrix R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] R[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3] R[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3] R[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2] R[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3] R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3] R[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1] R[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2] R[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1] R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3] self.rotMat = R #self.datalog_Att.write(str([time.time(), self.attitude]) + "\n") #print('Attitude r, p, y', self.attitude) def log_vel_callback(self, timestamp, data, Logconf): """ Callback for logging the velocity of the UAV defined w.r.t the body frame this subsequently rotated to the global frame""" PX, PY, PZ = [ data['kalman.statePX'], data['kalman.statePY'], data['kalman.statePZ'] ] R = self.rotMat self.velocity[0] = R[0][0]*PX + R[0][1]*PY + R[0][2]*PZ self.velocity[1] = R[1][0]*PX + R[1][1]*PY + R[1][2]*PZ self.velocity[2] = R[2][0]*PX + R[2][1]*PY + R[2][2]*PZ #self.datalog_Vel.write(str([time.time(), self.velocity]) + "\n") #print('Velocity rx, ry, rz', self.velocity) def log_mtr_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.Mtr = [ data['range.front'] * 0.001, data['range.back'] * 0.001, data['range.left'] * 0.001, data['range.right'] * 0.001, data['range.up'] * 0.001, ] #print('Time, Multirange: front, back, left, right, up', time.time(), self.Mtr) def _connection_failed(self, link_uri, msg): """ Callback when connection initial connection fails there is no Crazyflie at the specified address """ print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """ Callback when disconected after a connection has been made """ print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """ Callback when the Crazyflie is disconnected (called in all cases) """ if self.printDis: print('Disconnected from %s' % link_uri) self.printDis = False def loop_sleep(self, timeStart): """ Sleeps the control loop to make it run at a specified rate """ deltaTime = 1.0/float(self.rate) - (time.time() - timeStart) if deltaTime > 0: time.sleep(deltaTime) def _close_connection(self, message): """ This is able to close link connecting Crazyflie from keyboard """ if message == "q": self.isEnabled = False
class SyncCrazyflie: def __init__(self, link_uri, cf=None): """ Create a synchronous Crazyflie instance with the specified link_uri """ if cf: self.cf = cf else: self.cf = Crazyflie() self._link_uri = link_uri self._connect_event = Event() self._is_link_open = False self._error_message = None self.cf.connected.add_callback(self._connected) self.cf.connection_failed.add_callback(self._connection_failed) self.cf.disconnected.add_callback(self._disconnected) def open_link(self): if (self.is_link_open()): raise Exception('Link already open') print('Connecting to %s' % self._link_uri) self.cf.open_link(self._link_uri) self._connect_event.wait() if not self._is_link_open: raise Exception(self._error_message) def __enter__(self): self.open_link() return self def close_link(self): self.cf.close_link() self._is_link_open = False def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() def is_link_open(self): return self._is_link_open 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._is_link_open = True self._connect_event.set() 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._is_link_open = False self._error_message = msg self._connect_event.set() def _disconnected(self, link_uri): self._is_link_open = False
class OWExample: """ Simple example listing the 1-wire memories found and lists its contents. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # 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 self._mems_to_update = 0 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) mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) self._mems_to_update = len(mems) print("Found {} 1-wire memories".format(len(mems))) for m in mems: print("Updating id={}".format(m.id)) m.update(self._data_updated) def _data_updated(self, mem): print("Updated id={}".format(mem.id)) print("\tType : {}".format(mem.type)) print("\tSize : {}".format(mem.size)) print("\tValid : {}".format(mem.valid)) print("\tName : {}".format(mem.name)) print("\tVID : 0x{:02X}".format(mem.vid)) print("\tPID : 0x{:02X}".format(mem.pid)) print("\tPins : 0x{:02X}".format(mem.pins)) print("\tElements : ") for key in mem.elements: print("\t\t{}={}".format(key, mem.elements[key])) self._mems_to_update -= 1 if self._mems_to_update == 0: self._cf.close_link() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print("Error when logging %s: %s" % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print("[%d][%s]: %s" % (timestamp, logconf.name, data)) 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 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)) 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
class Main: def __init__(self): self._stop = 0 self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback( self.connectSetupFinished) def connectSetupFinished(self, linkURI): lg = LogConfig("Magnetometer", 100) lg.addVariable(LogVariable("mag.x", "int16_t")) lg.addVariable(LogVariable("mag.y", "int16_t")) lg.addVariable(LogVariable("mag.z", "int16_t")) log = self.crazyflie.log.create_log_packet(lg) if log is not None: log.dataReceived.add_callback(self.magData) log.start() else: print "Magnetometer not found in log TOC" # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() Thread(target=self.input_loop).start() def stop(self): # Exit command received # Set thrust to zero to make sure the motors turn off NOW # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1 # Exit the main loop time.sleep(1) self.crazyflie.close_link( ) # This errors out for some reason. Bad libusb? sys.exit(0) def input_loop(self): command = raw_input("") self.stop() def pulse_command(self): while 1: self.crazyflie.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) # Exit if the parent told us to if self._stop == 1: return def magData(self, data): x, y, z = data['mag.x'], data['mag.y'], data['mag.z'] try: print x, y, z sys.stdout.flush() except: self.stop()
class EEPROMExample: """ Simple example listing the EEPROMs found and writes the default values in it. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # 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) mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C) print("Found {} EEPOM(s)".format(len(mems))) if len(mems) > 0: print("Writing default configuration to" " memory {}".format(mems[0].id)) elems = mems[0].elements elems["version"] = 1 elems["pitch_trim"] = 0.0 elems["roll_trim"] = 0.0 elems["radio_channel"] = 80 elems["radio_speed"] = 0 elems["radio_address"] = 0xE7E7E7E7E7 mems[0].write_data(self._data_written) def _data_written(self, mem, addr): print("Data written, reading back...") mem.update(self._data_updated) def _data_updated(self, mem): print("Updated id={}".format(mem.id)) print("\tType : {}".format(mem.type)) print("\tSize : {}".format(mem.size)) print("\tValid : {}".format(mem.valid)) print("\tElements : ") for key in mem.elements: print("\t\t{}={}".format(key, mem.elements[key])) self._cf.close_link() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print("Error when logging %s: %s" % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print("[%d][%s]: %s" % (timestamp, logconf.name, data)) 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 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)) 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
class EEPROMExample: """ Simple example listing the EEPROMs found and lists its contents. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # 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) mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) print('Found {} 1-wire memories'.format(len(mems))) if len(mems) > 0: print('Writing test configuration to' ' memory {}'.format(mems[0].id)) mems[0].vid = 0xBC mems[0].pid = 0xFF board_name_id = OWElement.element_mapping[1] board_rev_id = OWElement.element_mapping[2] mems[0].elements[board_name_id] = 'Test board' mems[0].elements[board_rev_id] = 'A' mems[0].write_data(self._data_written) def _data_written(self, mem, addr): print('Data written, reading back...') mem.update(self._data_updated) def _data_updated(self, mem): print('Updated id={}'.format(mem.id)) print('\tType : {}'.format(mem.type)) print('\tSize : {}'.format(mem.size)) print('\tValid : {}'.format(mem.valid)) print('\tName : {}'.format(mem.name)) print('\tVID : 0x{:02X}'.format(mem.vid)) print('\tPID : 0x{:02X}'.format(mem.pid)) print('\tPins : 0x{:02X}'.format(mem.pins)) print('\tElements : ') for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) self._cf.close_link() def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) 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 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)) 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
class AltHoldExample: def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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.is_connected = True # Variable used to keep main loop occupied until disconnect 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.""" print('Connected to %s' % link_uri) # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._hover_test).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)) 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)) 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 def _hover_test(self): print("sending initial thrust of 0") self._cf.commander.send_setpoint(0,0,0,0); time.sleep(0.5); print("putting in althold") self._cf.param.set_value("flightmode.althold","True") print("Stay in althold for 7s") it=0 self._cf.commander.send_setpoint(0,0,0,40000) #self._cf.param.set_value("flightmode.althold","True") while it<300: #self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) #self._cf.commander.send_setpoint(0.66,-1,0,32767) self._cf.commander.send_setpoint(0,0,0,32767) self._cf.param.set_value("flightmode.althold","True") #self._cf.param.set_value("flightmode.poshold","False") time.sleep(0.01) it+=1 print("Close connection") self._cf.commander.send_setpoint(0,0,0,0) self._cf.close_link()
class CrazyflieROS: Disconnected = 0 Connecting = 1 Connected = 2 """Wrapper between ROS and Crazyflie SDK""" def __init__(self, link_uri, tf_prefix, roll_trim, pitch_trim, enable_logging): self.link_uri = link_uri self.tf_prefix = tf_prefix self.roll_trim = roll_trim self.pitch_trim = pitch_trim self.enable_logging = enable_logging self._cf = Crazyflie() 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.link_quality_updated.add_callback(self._link_quality_updated) self._cmdVel = Twist() self._subCmdVel = rospy.Subscriber(tf_prefix + "/cmd_vel", Twist, self._cmdVelChanged) self._pubImu = rospy.Publisher(tf_prefix + "/imu", Imu, queue_size=10) self._pubTemp = rospy.Publisher(tf_prefix + "/temperature", Temperature, queue_size=10) self._pubMag = rospy.Publisher(tf_prefix + "/magnetic_field", MagneticField, queue_size=10) self._pubPressure = rospy.Publisher(tf_prefix + "/pressure", Float32, queue_size=10) self._pubBattery = rospy.Publisher(tf_prefix + "/battery", Float32, queue_size=10) self._state = CrazyflieROS.Disconnected rospy.Service(tf_prefix + "/update_params", UpdateParams, self._update_params) rospy.Service(tf_prefix + "/emergency", Empty, self._emergency) self._isEmergency = False Thread(target=self._update).start() def _try_to_connect(self): rospy.loginfo("Connecting to %s" % self.link_uri) self._state = CrazyflieROS.Connecting self._cf.open_link(self.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.""" rospy.loginfo("Connected to %s" % link_uri) self._state = CrazyflieROS.Connected if self.enable_logging: self._lg_imu = LogConfig(name="IMU", period_in_ms=10) self._lg_imu.add_variable("acc.x", "float") self._lg_imu.add_variable("acc.y", "float") self._lg_imu.add_variable("acc.z", "float") self._lg_imu.add_variable("gyro.x", "float") self._lg_imu.add_variable("gyro.y", "float") self._lg_imu.add_variable("gyro.z", "float") self._cf.log.add_config(self._lg_imu) if self._lg_imu.valid: # This callback will receive the data self._lg_imu.data_received_cb.add_callback(self._log_data_imu) # This callback will be called on errors self._lg_imu.error_cb.add_callback(self._log_error) # Start the logging self._lg_imu.start() else: rospy.logfatal("Could not add logconfig since some variables are not in TOC") self._lg_log2 = LogConfig(name="LOG2", period_in_ms=100) self._lg_log2.add_variable("mag.x", "float") self._lg_log2.add_variable("mag.y", "float") self._lg_log2.add_variable("mag.z", "float") self._lg_log2.add_variable("baro.temp", "float") self._lg_log2.add_variable("baro.pressure", "float") self._lg_log2.add_variable("pm.vbat", "float") # self._lg_log2.add_variable("pm.state", "uint8_t") self._cf.log.add_config(self._lg_log2) if self._lg_log2.valid: # This callback will receive the data self._lg_log2.data_received_cb.add_callback(self._log_data_log2) # This callback will be called on errors self._lg_log2.error_cb.add_callback(self._log_error) # Start the logging self._lg_log2.start() else: rospy.logfatal("Could not add logconfig since some variables are not in TOC") # self._lg_log3 = LogConfig(name="LOG3", period_in_ms=100) # self._lg_log3.add_variable("motor.m1", "int32_t") # self._lg_log3.add_variable("motor.m2", "int32_t") # self._lg_log3.add_variable("motor.m3", "int32_t") # self._lg_log3.add_variable("motor.m4", "int32_t") # self._lg_log3.add_variable("stabalizer.pitch", "float") # self._lg_log3.add_variable("stabalizer.roll", "float") # self._lg_log3.add_variable("stabalizer.thrust", "uint16_") # self._lg_log3.add_variable("stabalizer.yaw", "float") # # self._cf.log.add_config(self._lg_log3) #if self._lg_log3.valid: # # This callback will receive the data # self._lg_log3.data_received_cb.add_callback(self._log_data_log3) # # This callback will be called on errors # self._lg_log3.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_log3.start() #else: # rospy.logfatal("Could not add logconfig since some variables are not in TOC") p_toc = self._cf.param.toc.toc for group in p_toc.keys(): self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback) for name in p_toc[group].keys(): ros_param = "/{}/{}/{}".format(self.tf_prefix, group, name) cf_param = "{}.{}".format(group, name) if rospy.has_param(ros_param): self._cf.param.set_value(cf_param, rospy.get_param(ros_param)) else: self._cf.param.request_param_update(cf_param) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" rospy.logfatal("Connection to %s failed: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" rospy.logfatal("Connection to %s lost: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" rospy.logfatal("Disconnected from %s" % link_uri) self._state = CrazyflieROS.Disconnected def _link_quality_updated(self, percentage): """Called when the link driver updates the link quality measurement""" if percentage < 80: rospy.logwarn("Connection quality is: %f" % (percentage)) def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" rospy.logfatal("Error when logging %s: %s" % (logconf.name, msg)) def _log_data_imu(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Imu() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" msg.orientation_covariance[0] = -1 # orientation not supported # measured in deg/s; need to convert to rad/s msg.angular_velocity.x = math.radians(data["gyro.x"]) msg.angular_velocity.y = math.radians(data["gyro.y"]) msg.angular_velocity.z = math.radians(data["gyro.z"]) # measured in mG; need to convert to m/s^2 msg.linear_acceleration.x = data["acc.x"] * 9.81 msg.linear_acceleration.y = data["acc.y"] * 9.81 msg.linear_acceleration.z = data["acc.z"] * 9.81 self._pubImu.publish(msg) #print "[%d][%s]: %s" % (timestamp, logconf.name, data) def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg) def _param_callback(self, name, value): ros_param = "{}/{}".format(self.tf_prefix, name.replace(".", "/")) rospy.set_param(ros_param, value) def _update_params(self, req): rospy.loginfo("Update parameters %s" % (str(req.params))) for param in req.params: ros_param = "/{}/{}".format(self.tf_prefix, param) cf_param = param.replace("/", ".") print(cf_param) #if rospy.has_param(ros_param): self._cf.param.set_value(cf_param, str(rospy.get_param(ros_param))) return UpdateParamsResponse() def _emergency(self, req): rospy.logfatal("Emergency requested!") self._isEmergency = True return EmptyResponse() def _send_setpoint(self): roll = self._cmdVel.linear.y + self.roll_trim pitch = self._cmdVel.linear.x + self.pitch_trim yawrate = self._cmdVel.angular.z thrust = min(max(0, int(self._cmdVel.linear.z)), 60000) #print(roll, pitch, yawrate, thrust) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) def _cmdVelChanged(self, data): self._cmdVel = data if not self._isEmergency: self._send_setpoint() def _update(self): while not rospy.is_shutdown(): if self._isEmergency: break if self._state == CrazyflieROS.Disconnected: self._try_to_connect() elif self._state == CrazyflieROS.Connected: # Crazyflie will shut down if we don't send any command for 500ms # Hence, make sure that we don't wait too long # However, if there is no connection anymore, we try to get the flie down if self._subCmdVel.get_num_connections() > 0: self._send_setpoint() else: self._cmdVel = Twist() self._send_setpoint() rospy.sleep(0.2) else: rospy.sleep(0.5) for i in range(0, 100): self._cf.commander.send_setpoint(0, 0, 0, 0) rospy.sleep(0.01) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing rospy.sleep(0.1) self._cf.close_link()
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() 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) 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._ramp_motors).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)) 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)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri) def take_off(self, roll, pitch, yawrate, thrust): x = 0 while x <= 4: #take off loop x = x + 1 self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000) time.sleep(0.1) return def hover(self, roll, pitch, yawrate, thrust): x = 0 while x <= 10: #hover Loop x = x + 1 self._cf.commander.send_setpoint(roll, pitch, yawrate, 32000) time.sleep(0.1) self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000) time.sleep(0.1) return def next_spot(self, roll, pitch, yawrate, thrust): x = 0 while x <= 3: #forward loop x = x + 1 self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000) time.sleep(0.1) #stop the quad from moving forward pitch = -2 self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000) time.sleep(0.1) pitch = 3 return def land(self, roll, pitch, yawrate, thrust): x = 0 thrust1 = 33000 while x <= 13: #landing loop x = x + 1 self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1) time.sleep(0.1) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1) time.sleep(0.1) thrust1 = thrust1 - 100 return def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 35000 pitch = 3 roll = 1 yawrate = 0 y = 0 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self.take_off(roll, pitch, yawrate, thrust) while y == 0: self.hover(roll, pitch, yawrate, thrust) pitch = 10 self.next_spot(roll, pitch, yawrate, thrust) pitch = 3 y = 1 self.hover(roll, pitch, yawrate, thrust) thrust = 33000 self.land(roll, pitch, yawrate, thrust) self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self._cf.close_link()
class formationControl(threading.Thread): """ A controller thread, taking references and mearsurements from the UAV computing a control input""" def __init__(self, link_uri, delta_ref, formation_fp, order_Cf2, num_Cf2): threading.Thread.__init__(self) #self.link_uri = link_uri self.daemon = True self.timePrint = 0.0 # Initialize the crazyflie object and add some callbacks 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.rate = 5 # (Hz) Rate of each sending control input self.statefb_rate = 200 # (ms) Time between two consecutive state feedback # Logged states -, attitude(p, r, y), rotation matrix self.position = [0.0, 0.0, 0.0] # [m] in the global frame of reference self.velocity = [0.0, 0.0, 0.0] # [m/s] in the global frame of reference self.attitude = [0.0, 0.0, 0.0] # [rad] attitude with inverted roll (r) self.rotMat = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] self.pos_other = ([0.0] * 3) * 1 self.formation_fp = formation_fp # Formation shape for each agent # Control setting self.isEnabled = True self.printDis = True # System parameter self.pi = 3.14159265359 self.delta_ref = delta_ref self.order_Cf2 = order_Cf2 self.num_Cf2 = num_Cf2 # Open text file to storage data self.myPos_log = open("log_data/Pos_log" + str(self.order_Cf2), "w") self.myInput_log = open("log_data/Input_log" + str(self.order_Cf2), "w") def _run_controller(self): """ Main control loop # System parameters""" timeSampling = 1.0/float(self.rate) # [s] sampling time delta = 1 # [] initial time, all Crazyflie lifted to 0.3m # Control Parameters # Reference parameter v0 = [0.0, 0.0, 0.0] # [m/s] reference velocity fp = [0.0, 0.0, 0.0] # [m] the formation shape fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape sh_start = 0.0 sh_end = 10.0 # Collision parameters col_thrs = 0.5 safe_thrs = 0.9 col_muy = 0.5 col_lda = 0.0 #dis_col = [0.0] * (self.num_Cf2 - 1) # Set the current reference to the current positional estimate time.sleep(3) # Unlock the controller # First, we need send a signal to enable sending attitude reference and thrust force self._cf.commander.send_setpoint(0, 0, 0, 0) # Hover at z = 0.3 durin 2 seconds x_0, y_0, z_0 = self.position yaw_0 = self.attitude[2] timeStart = time.time() while True: self._cf.commander.send_position_setpoint(x=x_0,y=y_0,z=0.4,yaw=yaw_0) time.sleep(0.2) if time.time() - timeStart > 4.0: break # Take a local coordinate x_0, y_0, z_0 = self.position yaw_d = self.attitude[2] print('Initial position reference: ({}, {}, {})'.format(x_0, y_0, z_0)) x_d, y_d, z_d = [0.0, 0.0, 0.0] # initial time pos_other = ([0.0] * 3) * 1 # Begin while loop for sending the control signal timeBeginCtrl = time.time() while True: timeStart = time.time() # Change the reference velocity if time.time() > timeBeginCtrl + 0.0: v0 = [0.0, 0.0, 0.0] delta = self.delta_ref if time.time() > timeBeginCtrl + 20.0: v0 = [0.02, -0.02, 0.0] if time.time() > timeBeginCtrl + 35.0: v0 = [-0.02, 0.02, 0.0] if time.time() > timeBeginCtrl + 50.0: v0 = [0.0, 0.0, 0.0] if time.time() > timeBeginCtrl + 60.0: self._cf.commander.send_setpoint(0, 0, 0, 40000) self._cf.close_link() print('Disconnect timeout') # Get the reference x_d = x_d + v0[0] * timeSampling y_d = y_d + v0[1] * timeSampling z_d = z_d + v0[2] * timeSampling # Get roll, pitch, yaw roll, pitch, yaw = self.attitude x, y , z = self.position vx, vy, vz = self.velocity # Get eta #eta_px = x - fp[0] #eta_py = y - fp[1] #eta_pz = z - fp[2] # A loop computes a potential function producing repulsive force u_x_c = 0 u_y_c = 0 u_z_c = 0 count_near = 0 for i in range(0, self.num_Cf2 - 1): # Get position from others pos_other = self.pos_other[i][0:3] # Collision avoidance e_col_x = x - pos_other[0] e_col_y = y - pos_other[1] e_col_z = z - pos_other[2] dis_col = sqrt(e_col_x*e_col_x + e_col_y*e_col_y + e_col_z*e_col_z) # collion-free force repul_force = self.bump_fcn_dot(dis_col,col_thrs,safe_thrs,col_muy,col_lda) u_x_c = u_x_c - repul_force * 2 * e_col_x #* vx u_y_c = u_y_c - repul_force * 2 * e_col_y #* vy #u_z_c = -repul_force * 2 * e_col_z #* vz u_z_c = u_z_c + u_x_c + u_y_c if repul_force != 0.0: count_near = count_near + 1 ## u_z_c = self.saturation(u_z_c, [-0.2, 0.2]) # More than 2 other Cf2 near this Cf2 if count_near > 1: u_z_c = 0.2 * (count_near - 1.0) # Setpoint x_d_c = x_0 + (x_d - x_0 + self.formation_fp[0]) * self.step_fcn(time.time(), timeBeginCtrl + sh_start, timeBeginCtrl + sh_end) + u_x_c y_d_c = y_0 + (y_d - y_0 + self.formation_fp[1]) * self.step_fcn(time.time(), timeBeginCtrl + sh_start, timeBeginCtrl + sh_end) + u_y_c # Send set point self._cf.commander.send_position_setpoint(x_d_c,y_d_c,0.4+u_z_c,yaw_d) #if self.order_Cf2 == 0: # self._cf.commander.send_position_setpoint(x_d_c,y_d_c,0.4,yaw_d) #else: # self._cf.commander.send_setpoint(0, 0, 0, 20000) ## message = ('ref:({}, {}, {})\n'.format(x_d_c, y_d_c, z_d) + 'pos:({}, {}, {})\n'.format(x, y, z) + 'pos_other:({}, {}, {})\n'.format(pos_other[0], pos_other[1], pos_other[2]) + 'dis:({})\n'.format(dis_col)) message1 = ('rep_force :({}, {}, {})\n'.format(u_x_c, u_y_c, u_z_c)) if self.order_Cf2 == 0: self.print_at_period(0.5, message1) # Storage data self.myPos_log.write(str(time.time()) + "," + str(x) + "," + str(y) + "," + str(z) + "\n") #self.myInput_log.write(str(time.time()) + "," + # str(u_x) + "," + # str(u_y) + "," + # str(u_z) + "\n") self.loop_sleep(timeStart) # End def saturation(self, value, limits): """ Saturate a given value within limit interval""" if value < limits[0]: value = limits[0] #print("limit low") elif value > limits[1]: value = limits[1] #print("limit up") return value def bump_fcn(self, mr, threshold, muy): """ This is bump function""" if mr > threshold: mr_bump = 0 else: mr_bump = (threshold - mr) * (threshold - mr) mr_bump = mr_bump/(mr + threshold * threshold * 1.0/muy) return mr_bump def bump_fcn_dot(self, mr, col_thrs, safe_thrs, muy, lda): """ This is the derivative of the bump function""" if mr > col_thrs: mr_bump_dot = lda * self.step_dot_fcn(mr, col_thrs, safe_thrs) else: mr_bump_dot = float(mr) + 2.0 * col_thrs * col_thrs / muy + col_thrs mr_bump_dot = - mr_bump_dot * (col_thrs - float(mr)) mr_bump_dot = mr_bump_dot / ((float(mr) + col_thrs * col_thrs / muy)*(float(mr) + col_thrs * col_thrs / muy)) return mr_bump_dot def step_fcn(self, mr, low, up): """ This is smooth step function """ if mr <= low: step_out = 0 elif mr > low and mr < up: step_out = (float(mr) - low)/(up - low) step_out = step_out * step_out else: step_out = 1 return step_out def step_dot_fcn(self, mr, low, up): """ This is smooth step function """ if mr <= low or mr >= up: step_out = 0 else: step_out = 2*(float(mr) - low)/(up - low) step_out = step_out / (up - low) return step_out def print_at_period(self, period, message): """ Prints the message at a given period""" if (time.time() - period) > self.timePrint: self.timePrint = time.time() print(message) def _connected(self, link_uri): """ This callback is called from the Crazyflie API when a Crazyflie has been connected adn TOCs have been downloaded """ print('connected to: %s' %link_uri) # Reset Kalman filter self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) print('Wait for Kalamn filter') # Feedback position estimated by KF logPos = LogConfig(name='Kalman Position', period_in_ms=self.statefb_rate) logPos.add_variable('kalman.stateX','float') logPos.add_variable('kalman.stateY','float') logPos.add_variable('kalman.stateZ','float') # Feedback velocity estimated by KF logVel = LogConfig(name='Kalman Velocity', period_in_ms=self.statefb_rate ) logVel.add_variable('kalman.statePX','float') logVel.add_variable('kalman.statePY','float') logVel.add_variable('kalman.statePZ','float') # Feedback Quaternion attitude estimated by KF logAtt = LogConfig(name='Kalman Attitude', period_in_ms=self.statefb_rate ) logAtt.add_variable('kalman.q0','float') logAtt.add_variable('kalman.q1','float') logAtt.add_variable('kalman.q2','float') logAtt.add_variable('kalman.q3','float') # Invoke logged states self._cf.log.add_config(logPos) self._cf.log.add_config(logVel) self._cf.log.add_config(logAtt) # Start invoking logged states if logPos.valid: # Position logPos.data_received_cb.add_callback(self.log_pos_callback) logPos.error_cb.add_callback(logging_error) logPos.start() # Velocity logVel.data_received_cb.add_callback(self.log_vel_callback) logVel.error_cb.add_callback(logging_error) logVel.start() # Quadternion attitude logAtt.data_received_cb.add_callback(self.log_att_callback) logAtt.error_cb.add_callback(logging_error) logAtt.start() else: print("One or more of the variables in the configuration was not found"+ "in log TOC. No logging will be possible") # Start in a thread threading.Thread(target=self._run_controller).start() def log_pos_callback(self, timestamp, data, Logconf): """ Callback for the logging the position of the UAV in the global frame""" self.position = [ data['kalman.stateX'], data['kalman.stateY'], data['kalman.stateZ'] ] #self.myPos_log.write(str(time.time()) + "," + # str(self.position[0]) + "," + # str(self.position[1]) + "," + # str(self.position[2]) + "\n") #print('Position x, y, z, time', self.position, time.time()) #print(self.rate) def log_att_callback(self, timestamp, data, Logconf): """ Callback for the logging the quadternion attitude of the UAV which is converted to roll, pitch, yaw in radians""" q = [ data['kalman.q0'], data['kalman.q1'], data['kalman.q2'], data['kalman.q3'] ] #Convert the quadternion to pitch roll and yaw yaw = atan2(2*(q[1]*q[2]+q[0]*q[3]) , q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) pitch = asin(-2*(q[1]*q[3] - q[0]*q[2])) roll = atan2(2*(q[2]*q[3]+q[0]*q[1]) , q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) self.attitude = [roll, pitch, yaw] # Convert the quaternion to a rotation matrix R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] R[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3] R[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3] R[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2] R[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3] R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3] R[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1] R[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2] R[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1] R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3] self.rotMat = R #self.datalog_Att.write(str([time.time(), self.attitude]) + "\n") #print('Attitude r, p, y', self.attitude) def log_vel_callback(self, timestamp, data, Logconf): """ Callback for logging the velocity of the UAV defined w.r.t the body frame this subsequently rotated to the global frame""" PX, PY, PZ = [ data['kalman.statePX'], data['kalman.statePY'], data['kalman.statePZ'] ] R = self.rotMat self.velocity[0] = R[0][0]*PX + R[0][1]*PY + R[0][2]*PZ self.velocity[1] = R[1][0]*PX + R[1][1]*PY + R[1][2]*PZ self.velocity[2] = R[2][0]*PX + R[2][1]*PY + R[2][2]*PZ def _connection_failed(self, link_uri, msg): """ Callback when connection initial connection fails there is no Crazyflie at the specified address """ print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """ Callback when disconected after a connection has been made """ print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """ Callback when the Crazyflie is disconnected (called in all cases) """ if self.printDis: print('Disconnected from %s' % link_uri) self.printDis = False def loop_sleep(self, timeStart): """ Sleeps the control loop to make it run at a specified rate """ deltaTime = 1.0/float(self.rate) - (time.time() - timeStart) if deltaTime > 0: time.sleep(deltaTime) def _close_connection(self, message): """ This is able to close link connecting Crazyflie from keyboard """ if message == "q": self.isEnabled = False # end def subscribe_other_infor(self, pos_other): """ This function updates the information including eta_p and eta_v from other Crazyflies """ self.pos_other = pos_other def get_publish_infor(self): """ This calback take the information of this Crazyflie""" return self.position
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers( enable_debug_driver=GuiConfig().get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked( GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about( self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get("device_config_mapping")[ self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text( ) GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions( )[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text( ) # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
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')