class TestFlight: def __init__(self): """ Initialize the quad copter """ # Log file for Accelerometer self.f1 = open('Accelerometer_log.txt', 'w') # Log file for Pitch Roll Yaw self.f2 = open('Stabalizer_log.txt', 'w') # Log file for altitude self.f3 = open('Barometer_log.txt', 'w') # write data into the log files self.f1.write('Accelerometer log\n {date} {acc.x} {acc.y} {acc.z}\n') self.f2.write('Stabilizer log\n{date} {Roll} {Pitch} {Yaw} {Thrust}\n') self.f3.write('Barometer log\n {data} {ASL}\n') # get the Unix time self.starttime = time.time()*1000.0 self.date = time.time()*1000.0 - self.starttime # Initialize the crazyflie and get the drivers ready self.crazyflie = cflib.crazyflie.Crazyflie() print 'Initializing drivers' cflib.crtp.init_drivers() # Start scanning available devices print 'Searching for available devices' available = cflib.crtp.scan_interfaces() radio = False for i in available: # Connect to the first device of the type 'radio' if 'radio' in i[0]: radio = True dev = i[0] print 'Connecting to interface with URI [{0}] and name {1}'.format(i[0], i[1]) self.crazyflie.open_link("radio://0/80/250K") ## radio://0/80/250K is most well connected radio frequency # Heungseok Park, 4.9.2015 #self.crazyflie.open_link(dev) break if not radio: print 'No quadcopter detected. Try to connect again.' exit(-1) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): """ Set the loggers """ """ # Log stabilizer self.logStab = LogConfig("Stabalizer", 200) self.logStab.add_variable("stabilizer.roll", "float") self.logStab.add_variable("stabilizer.pitch", "float") self.logStab.add_variable("stabilizer.yaw", "float") self.logStab.add_variable("stabilizer.thrust", "uint16_t") self.crazyflie.log.add_config(self.logStab) if self.logStab.valid: self.logStab.data_received_cb.add_callback(self.print_stab_data) self.logStab.start() else: print 'Could not setup log configuration for stabilizer after connection!' """ # Log barometer # we use only barometer value(ASL Value) to control altitude self.logBaro = LogConfig("Baro", 200) self.logBaro.add_variable("baro.aslLong", "float") self.crazyflie.log.add_config(self.logBaro) if self.logBaro.valid: self.logBaro.data_received_cb.add_callback(self.print_baro_data) self.logBaro.start() else: print 'Could not setup log configuration for barometer after connection!' """ # Log Accelerometer self.logAccel = LogConfig("Accel",200) self.logAccel.add_variable("acc.x", "float") self.logAccel.add_variable("acc.y", "float") self.logAccel.add_variable("acc.z", "float") self.crazyflie.log.add_config(self.logAccel) if self.logAccel.valid: self.logAccel.data_received_cb.add_callback(self.print_accel_data) self.logAccel.start() else: print 'Could not setup log configuration for accelerometer after connection!' """ # Start another thread and doing control function call print "log for debugging: before start increasing_step" # Thread(target=self.increasing_step).start() # Thread(target=self.recursive_step).start() Thread(target=self.init_alt).start() def print_baro_data(self, ident, data, logconfig): # Output the Barometer data #logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"])) # global variable that holds current altitude global current_alt current_alt = data["baro.aslLong"] # system output the time and the altitude, each id represents a time slice in Unix date = time.time()*1000.0 - self.starttime sys.stdout.write('Id={0}, Baro: baro.aslLong{1:.4f}\r\n'.format(ident, data["baro.aslLong"])) self.f3.write('{} {}\n'.format(date, data["baro.aslLong"])) pass def print_stab_data(self, ident, data, logconfig): # Output the stablizer data (roll pith yaw) sys.stdout.write('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"])) # print('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"])) self.f2.write('{} {} {} {} {}\n'.format(self.date, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"])) def print_accel_data(self, ident, data, logconfig): # Output the accelerometer data # global variables that holds x,y,z value global accelvaluesX global accelvaluesY global accelvaluesZ sys.stdout.write('Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\r".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))') #print("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\n".format(ident, data["acc.x"], data["acc.y"], data["acc.z"])) self.f1.write('{} {} {} {}\n'.format(self.date, data["acc.x"], data["acc.y"], data["acc.z"])) #sys.stdout.write("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\r".format(ident, data["acc.x"], data["acc.y"], data["acc.z"])) #date = time.time()*1000.0 - self.starttime #small_array = [] #small_array.append(date) #small_array.append(data["acc.x"]) #small_array.append(data["acc.y"]) #small_array.append(data["acc.z"]) #accelvaluesX.append(small_array_x) #print(accelvaluesX) #print(accelvaluesY) #print(accelvaluesZ) def init_alt(self): #global current_alt global target_alt global current_alt current_temp = current_alt target_temp = target_alt #print "- current_temp" + current_temp #print "- target_temp" + target_temp # If the current < target the thrust up gain altitude # round function change float value to int if(round(current_temp) < target_temp): sys.stdout.write("Current alt is lower than target value, Let's go up!\r\n") self.crazyflie.commander.send_setpoint(0, 0, 0, 43000) #for bandwidth reason, the command need to be delayed time.sleep(0.5) return self.init_alt() # If the current > target the thrust down lose altitude elif(round(current_temp) > target_temp): sys.stdout.write("Current alt is higher than target value, Let's go down!\r\n") self.crazyflie.commander.send_setpoint(0, 0, 0, 32000) #for bandwidth reason, the command need to be delayed time.sleep(0.2) return self.init_alt() # If the current = target then hold the altitude by using the build-in function althold elif(round(current_temp) == target_temp): sys.stdout.write("Now, current and target altitude is same, Let's hover!\r\n") self.crazyflie.param.set_value("flightmode.althold", "True") #the althold will last 4 second and then we set the thrust to 32767 which is the value that will hover self.crazyflie.commander.send_setpoint(0, 0, 0, 32767) def recursive_step(self, altc, altt): # this function pass the crazyflie current altitude as well as target altitude # it recursivly call it self in 3 situations # Post condition; the drone reach to the target altitude and hover # Temp variables to hold the parameter current_temp = altc["baro.aslLong"] target_temp = altt # If the current < target the thrust up gain altitude if(current_temp < target_temp): sys.stdout.write("Current alt is lower than target value, Let's go up!\r\n") self.crazyflie.commander.send_setpoint(0, 0, 0, 40000) current_temp = altc["baro.aslLong"] return self.recursive_step(current_temp, target_temp) # If the current > target the thrust down lose altitude elif(current_temp > target_temp): sys.stdout.write("Currnet alt is higher than target value, Let's go down!\r\n") self.crazyflie.commander.send_setpoint(0, 0, 0, 30000) current_temp= altc["baro.aslLong"] return self.recursive_step(current_temp, target_temp) # If the current = target then hold the altitude by using the build-in function althold elif(current_temp == target_temp): sys.stdout.write("Now, current and target altitude is same, Let's hover!\r\n") self.crazyflie.param.set_value("flightmode.althold", "True") return def increasing_step(self): # This function reads the key, and different key input indicates different output # If you use global var, you need to modify global copy global key global accelvaluesX global accelvaluesY global accelvaluesZ global current_alt # Debug for the current altitude print(current_alt) # now, this will print 0 # (blades start to rotate after 10000 #initialize the var # Thrust init start_thrust = 43000 min_thrust = 10000 max_thrust = 60000 thrust_increment = 3000 # Flag is for the altitude hold mode flag = True # Roll init start_roll = 0 roll_increment = 30 min_roll = -50 max_roll = 50 # Pitch init start_pitch = 0 pitch_increment = 30 min_pitch = -50 max_pitch = 50 # Yaw init start_yaw = 0 yaw_increment = 30 min_yaw = -200 max_yaw = 200 stop_moving_count = 0 # Target init pitch = start_pitch roll = start_roll thrust = start_thrust yaw = start_yaw #unlock the thrust protection self.crazyflie.commander.send_setpoint(0,0,0,0) # Start the keyread thread keyreader = KeyReaderThread() keyreader.start() sys.stdout.write('\r\nCrazyflie Status\r\n') sys.stdout.write('================\r\n') sys.stdout.write("Use 'w' and 's' for the thrust, 'a' and 'd' for yaw, 'i' and 'k' for pitch and 'j' and 'l' for roll. Stop flying with 'q'. Exit with 'e'.\r\n") #g = Gnuplot.Gnuplot(debug=1) #g.title('A simple example') # (optional) #g('set data style line') # give gnuplot an arbitrary command # Plot a list of (x, y) pairs (tuples or a numpy array would # also be OK): #g.plot(accelvaluesX) # key e is to exit the program while key != "e": # key q is to kill the drone if key == 'q': #thrust = pitch = roll = yaw = 0 print "killing the drone" thrust = 0 pitch = 0 roll = 0 yaw = 0 # key w is to increase the thrust elif key == 'w' and (thrust + thrust_increment <= max_thrust): thrust += thrust_increment print "thrust: " print thrust # key s is to decrease the thrust elif key == 's' and (thrust - thrust_increment >= min_thrust): thrust -= thrust_increment print "thrust: " print thrust # key d is to increase the yaw elif key == 'd' and (yaw + yaw_increment <= max_yaw): yaw += yaw_increment stop_moving_count = 0 # key a is to decrease the yaw elif key == 'a' and (yaw - yaw_increment >= min_yaw): yaw -= yaw_increment stop_moving_count = 0 # key l is to increase the roll elif key == 'l' and (roll + roll_increment <= max_roll): roll += roll_increment stop_moving_count = 0 # key j is to decrease the roll elif key == 'j' and (roll - roll_increment >= min_roll): roll -= roll_increment stop_moving_count = 0 # key i is to increase the pitch elif key == 'i' and (pitch + pitch_increment <= max_pitch): pitch += pitch_increment stop_moving_count = 0 # key k is to decrease the pitch elif key == 'k' and (pitch - pitch_increment >= min_pitch): pitch -= pitch_increment stop_moving_count = 0 # 'h' is altitude hold mode elif key == 'h': # flag is initialized as true if flag == True: self.crazyflie.param.set_value("flightmode.althold", "True") sys.stdout.write("althold mode\r\n") print "thrust: " print thrust flag = False else: self.crazyflie.param.set_value("flightmode.althold", "False") sys.stdout.write("standard mode\r\n") print "thrust: " print thrust flag = True #elif key == 'x': # if the user did not input the keys listed then it count until 40 # then kill the drone elif key == '': if stop_moving_count >= 40: pitch = 0 roll = 0 yaw = 0 else: stop_moving_count += 1 else: pass key = '' self.crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing self.crazyflie.commander.send_setpoint(0,0,0,0) self.crazyflie.close_link()
class Quad(threading.Thread): """ 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 """ threading.Thread.__init__(self) # 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 Thread(target=self._control).start() Thread(target=self._input).start() ######################################################################## # Set up Stabilizer Logger ######################################################################## self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50) self._lg_stab.add_variable("stabilizer.roll", "float") self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._log_stab_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._log_error) # Start the logging self._lg_stab.start() else: print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Acc Logger # ######################################################################## # self._lg_acc = LogConfig(name="Acc", period_in_ms=50) # self._lg_acc.add_variable("acc.x", "float") # self._lg_acc.add_variable("acc.y", "float") # self._lg_acc.add_variable("acc.z", "float") # self._cf.log.add_config(self._lg_acc) # if self._lg_acc.valid: # # This callback will receive the data # self._lg_acc.data_received_cb.add_callback(self._log_acc_data) # # This callback will be called on errors # self._lg_acc.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_acc.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Gyro Logger # ######################################################################## # self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50) # self._lg_gyro.add_variable("gyro.x", "float") # self._lg_gyro.add_variable("gyro.y", "float") # self._lg_gyro.add_variable("gyro.z", "float") # self._cf.log.add_config(self._lg_gyro) # if self._lg_gyro.valid: # # This callback will receive the data # self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data) # # This callback will be called on errors # self._lg_gyro.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_gyro.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Baro Logger # ######################################################################## self._lg_baro = LogConfig(name="Baro", period_in_ms=50) self._lg_baro.add_variable("baro.aslLong", "float") self._cf.log.add_config(self._lg_baro) if self._lg_baro.valid: # This callback will receive the data self._lg_baro.data_received_cb.add_callback(self._log_baro_data) # This callback will be called on errors self._lg_baro.error_cb.add_callback(self._log_error) # Start the logging self._lg_baro.start() else: print("Could not add logconfig since some variables are not in TOC") 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 _log_stab_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_roll, g_yaw, g_pitch, g_thrust (g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"]) # def _log_acc_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_acc_x, g_acc_y, g_acc_z; # (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"]) # def _log_gyro_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_gyro_x, g_gyro_y, g_gyro_z; # (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"]) def _log_baro_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_baro; g_baro = data["baro.aslLong"] def _control(self): global g_roll, g_yaw, g_pitch, g_thrust global g_gyro_x, g_gyro_y, g_gyro_z global g_acc_x, g_acc_y, g_acc_z global g_baro, g_init_baro, g_kill global diff_baro global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax global count print "Control Started" while not g_kill: #print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z), #print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z), #print "thrust: %d" % thrust, print "difference: %10f" % diff_baro, print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro), print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch) if not (g_init_baro == 0): diff_baro = g_init_baro - g_baro roll = 0 pitch = 0 yawrate = 0 count = count + 1 if diff_baro > 0.0: thrust = thrustInit + scaleUp * diff_baro else: thrust = thrustInit + scaleDown * diff_baro if thrust < thrustMin: thrust = thrustMin if thrust > thrustMax: thrust = thrustMax self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.01) # thrust_mult = 1 # thrust_step = 500 # thrust = 20000 # pitch = 0 # roll = 0 # yawrate = 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 self._cf.close_link() def _input(self): global g_baro, g_init_baro, g_kill print "Input Started" getchVar = _Getch() while g_kill is False: if (getchVar == 's'): g_init_baro = g_baro-1 elif (getchVar == 'k'): self._cf.commander.send_setpoint(0,0,0,0) g_kill = True print "KILLED!!!" #while g_kill is False: # command = raw_input("Input Command:\n") # if (command[0] == 'S') or (command[0] == 's'): # g_init_baro = g_baro-1 # elif (command[0] == 'K') or (command[0] == 'k'): # self._cf.commander.send_setpoint(0, 0, 0, 0) # g_kill = True # print "KILLED!!!" #def setup_term(fd, when=termios.TCSAFLUSH): # mode = termios.tcgetattr(fd) # mode[tty.LFLAG] = mode[tty.LFLAG] & ~(termios.ECHO | termios.ICANON) # termios.tcsetattr(fd, when, mode) #def getch(timeout=None): # fd = sys.stdin.fileno() # old_settings = termios.tcgetattr(fd) # try: # setup_term(fd) # try: # rw, wl, xl = select.select([fd], [], [], timeout) # except select.error: # return # if rw: # return sys.stdin.read(1) # finally: # termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 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 Quad: """ 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 """ # 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 Thread(target=self._control).start() ######################################################################## # Set up Stabilizer Logger ######################################################################## 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") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._log_stab_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._log_error) # Start the logging self._lg_stab.start() else: print("Could not add logconfig since some variables are not in TOC") ######################################################################## # Set up Acc Logger ######################################################################## self._lg_acc = LogConfig(name="Acc", period_in_ms=10) self._lg_acc.add_variable("acc.x", "float") self._lg_acc.add_variable("acc.y", "float") self._lg_acc.add_variable("acc.z", "float") self._cf.log.add_config(self._lg_acc) if self._lg_acc.valid: # This callback will receive the data self._lg_acc.data_received_cb.add_callback(self._log_acc_data) # This callback will be called on errors self._lg_acc.error_cb.add_callback(self._log_error) # Start the logging self._lg_acc.start() else: print("Could not add logconfig since some variables are not in TOC") ######################################################################## # Set up Gyro Logger ######################################################################## self._lg_gyro = LogConfig(name="Gyro", period_in_ms=10) self._lg_gyro.add_variable("gyro.x", "float") self._lg_gyro.add_variable("gyro.y", "float") self._lg_gyro.add_variable("gyro.z", "float") self._cf.log.add_config(self._lg_gyro) if self._lg_gyro.valid: # This callback will receive the data self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data) # This callback will be called on errors self._lg_gyro.error_cb.add_callback(self._log_error) # Start the logging self._lg_gyro.start() else: print("Could not add logconfig since some variables are not in TOC") def _control(self): global g_roll, g_yaw, g_pitch; global g_acc_x, g_acc_y, g_acc_z; while True: print "Acc : x = %10f, y = %10f, z = %10f" % (g_acc_x, g_acc_y, g_acc_z) print "Gyro : x = %10f, y = %10f, z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z) # print "Stab : roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch) time.sleep(0.1) # thrust_mult = 1 # thrust_step = 500 # thrust = 20000 # pitch = 0 # roll = 0 # yawrate = 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() 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 _log_stab_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_roll, g_yaw, g_pitch; (g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"]) def _log_acc_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_acc_x, g_acc_y, g_acc_z; (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"]) def _log_gyro_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_gyro_x, g_gyro_y, g_gyro_z; (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"]) 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: # Initial values, you can use these to set trim etc. roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 10001 # desire asl value, it could be changed current_asl_value + 50 d_asl = 685 def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers(enable_debug_driver=False) print "Scanning interfaces for Crazyflies..." available = cflib.crtp.scan_interfaces() print "Crazyflies found: " for i in available: print i[0] # You may need to update this value if your Crazyradio uses a different frequency. link_uri = available[0][0] #link_uri = available[0][0] #self.crazyflie.open_link(link_uri) self.crazyflie.open_link("radio://0/80/250K") 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): # Keep the commands alive so the firmware kill-switch doesn't kick in. Thread(target=self.pulse_command).start() hold_flag = True # The definition of the logconfig can be made before connecting self._lg_alt = LogConfig(name="altitude", period_in_ms=10) self._lg_alt.add_variable("baro.asl", "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. self._cf.log.add_config(self._lg_alt) if self._lg_alt.valid: # This callback will receive the data self._lg_alt.data_received_cb.add_callback(self._alt_log_data) # This callback will be called on errors self._lg_alt.error_cb.add_callback(self._alt_log_error) # Start the logging self._lg_alt.start() else: print("Could not add logconfig since some variables are not in TOC") while 1: #self.crazyflie.log. self.thrust = int(raw_input("Set thrust (10001-60000):")) if self.thrust == 0: self.crazyflie.close_link() break elif self.thrust <= 10000: self.thrust = 10001 elif self.thrust == 40000: # Test altitude hold flightmode time.sleep(10) #self.crazyflie.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.05) self.param.set_value("flightmode.althold", "True") #time.sleep(10) #self.param.set_value("flightmode.althold", "False") elif self.thrust > 60000: self.thrust = 60000 def _alt_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) def _alt_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" if logconf.name == "altitude": if not self._takeoff: self._start_alt = data['baro.asl'] self._takeoff = True else: self._alt = data['baro.asl'] print "{}".format(self._alt - self._start_alt) def pulse_command(self): self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust) time.sleep(0.1) # ..and go again! self.pulse_command()
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 TestFlight: def __init__(self): """ Initialize the quadcopter """ self.crazyflie = cflib.crazyflie.Crazyflie() logging.info("Initializing drivers") cflib.crtp.init_drivers() logging.info("Searching for available devices") available = cflib.crtp.scan_interfaces() radio = False for i in available: # Connect to the first device of the type 'radio' if 'radio' in i[0]: radio = True dev = i[0] logging.info("Connecting to interface with URI [{0}] and name {1}".format(i[0], i[1])) self.crazyflie.open_link(dev) break if not radio: logging.info("No quadcopter detected. Try to connect again.") exit(-1) # Set up the callback when connected self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): """ Set the loggers """ # Log stabilizer self.logStab = LogConfig("Stabalizer", 200) self.logStab.add_variable("stabilizer.roll", "float") self.logStab.add_variable("stabilizer.pitch", "float") self.logStab.add_variable("stabilizer.yaw", "float") self.logStab.add_variable("stabilizer.thrust", "uint16_t") self.crazyflie.log.add_config(self.logStab) if self.logStab.valid: self.logStab.data_received_cb.add_callback(self.print_stab_data) self.logStab.start() else: logger.warning("Could not setup log configuration for stabilizer after connection!") # Log barometer self.logBaro = LogConfig("Baro", 200) self.logBaro.add_variable("baro.aslLong", "float") self.crazyflie.log.add_config(self.logBaro) if self.logBaro.valid: self.logBaro.data_received_cb.add_callback(self.print_baro_data) self.logBaro.start() else: logger.warning("Could not setup log configuration for barometer after connection!") # Log Accelerometer self.logAccel = LogConfig("Accel",200) self.logAccel.add_variable("acc.x", "float") self.logAccel.add_variable("acc.y", "float") self.logAccel.add_variable("acc.z", "float") self.crazyflie.log.add_config(self.logAccel) if self.logAccel.valid: self.logAccel.data_received_cb.add_callback(self.print_accel_data) self.logAccel.start() else: logger.warning("Could not setup log configuration for accelerometer after connection!") # Call the requested thrust profile. # Start a separate thread to run test. # Do not hijack the calling thread! if args.thrust_profile == 'increasing_step': Thread(target=self.increasing_step).start() if args.thrust_profile == 'prbs_hover': Thread(target=self.prbs_hover).start() if args.thrust_profile == 'prbs_asc': Thread(target=self.prbs_asc).start() if args.thrust_profile == 'prbs_desc': Thread(target=self.prbs_desc).start() def print_baro_data(self, ident, data, logconfig): logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"])) def print_stab_data(self, ident, data, logconfig): logging.info("Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}".format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"])) def print_accel_data(self, ident, data, logconfig): logging.info("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}".format(ident, data["acc.x"], data["acc.y"], data["acc.z"])) # Thrust Profiles def increasing_step(self): thrust = 30000 pitch = 0 roll = 0 yaw_rate = 0 self.crazyflie.commander.send_setpoint(roll, pitch, yaw_rate, thrust) 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() def prbs_hover(self): pass def prbs_asc(self): pass def prbs_desc(self): pass
def get_accelerometer_log_config(self): log_conf = LogConfig("Accel", period_in_ms=100) log_conf.add_variable("acc.x", "float") log_conf.add_variable("acc.y", "float") log_conf.add_variable("acc.z", "float") return log_conf
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 HoverTest: """Example that connects to the first Crazyflie found, hovers, and 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) self._status = {} self._status['stabilizer.roll'] = 0 self._status['stabilizer.thrust'] = 0 self._status['stabilizer.yaw'] = 0 self._status['stabilizer.pitch'] = 0 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) # The definition of the logconfig can be made before connecting self._lg_stabilizer = LogConfig(name="stabilizer", period_in_ms=10) self._lg_stabilizer.add_variable("stabilizer.roll", "float") self._lg_stabilizer.add_variable("stabilizer.thrust", "uint16_t") self._lg_stabilizer.add_variable("stabilizer.yaw", "float") self._lg_stabilizer.add_variable("stabilizer.pitch", "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_stabilizer) # This callback will receive the data self._lg_stabilizer.data_received_cb.add_callback(self._stabilizer_log_data) # This callback will be called on errors self._lg_stabilizer.error_cb.add_callback(self._stabilizer_log_error) # Start the logging self._lg_stabilizer.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.") # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._hover).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 _stabilizer_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print("Error when logging %s: %s" % (logconf.name, msg)) def _stabilizer_log_data(self, timestamp, data, logconf): #print("[%d][%s]: %s" % (timestamp, logconf.name, data)) self._status['stabilizer.roll'] = data['stabilizer.roll'] self._status['stabilizer.thrust'] = data['stabilizer.thrust'] self._status['stabilizer.yaw'] = data['stabilizer.yaw'] self._status['stabilizer.pitch'] = data['stabilizer.pitch'] def _hover(self): global mpitch, mthrust, myaw, mroll, run # Unlock startup thrust protection. #self._cf.commander.send_setpoint(0, 0, 0, 0) # Turn on altitude hold. #self._cf.param.set_value("flightmode.althold", "True") #f = open("Log.txt", "w") self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.5) print "Unlocked" while run: # Update the position. #print("Setting Point: ") if ((mthrust >= minthrust)&(mgrab == 0)): self._cf.commander.send_setpoint(mroll, mpitch, myaw, mthrust) time.sleep(0.01) else: self._cf.commander.send_setpoint(0, 0, 0, 0) #turns off wothout delay and initially unlocks the thrust print "zero" print " pitch: %6.2f %6.2f, roll: %6.2f %6.2f, yaw: %6.2f %6.2f, mthrust: %6.2f %6.2f, grab: %2f" % ( mpitch, self._status['stabilizer.pitch'], mroll, self._status['stabilizer.roll'], myaw, self._status['stabilizer.yaw'], mthrust, self._status['stabilizer.thrust'], mgrab) #f.write("%f %f %f %f\n" % (self._status['stabilizer.roll'], self._status['stabilizer.pitch'], mroll, mpitch)) #self._cf.commander.send_setpoint(0, 0, 0, 0) #f.close() # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) print "run %f" % run time.sleep(0.1) self._cf.close_link()
class HoverExample: def __init__(self, link_uri): """ global loop vars """ self._exit = False """ camera setup """ cam_ip = raw_input("Specify camera; enter for webcam, or ip of network :\n") if cam_ip is '': self._cam = SimpleCV.Camera() elif '.' not in cam_ip: self._cam = SimpleCV.JpegStreamCamera("http://192.168.1." + cam_ip + ":8080/video") else: self._cam = SimpleCV.JpegStreamCamera("http://" + cam_ip + ":8080/video") self._img_size = (800,600) self._img_set_point_accepted = False self._img_set_point = 0 self._img_color = SimpleCV.Color.RED self._img_blob_min = 10 self._img_blob_max = 1000 self._img_log_path = '/tmp/hover.img.log' # start the vision loop -- will need to initialize though # Thread(target=self._vision_loop).start() #raw_input("Place drone at set point; then press enter:\n") self._img_set_point_accepted = True #self._img_set_point = self._img_pos.y self._img_set_point = 480 raw_input("Set-point is {0}; press any key to conintue.\n".format(self._img_set_point)) """ 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._hover_target = 1 self._flight_time = 10 self._flight_log_path = "/tmp/hover.log" self._asl = 0 self._alt_accepted = False print "Connecting to %s" % link_uri self._cf.open_link(link_uri) # raw_input("Acquiring altitude reading. Press any key to accept\n") self._alt_accepted = True raw_input("Beginning hover. Press any key to stop\n") self._exit = True def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print "Connected to %s" % link_uri self._log = LogConfig(name="Stabilizer", period_in_ms=10) self._log.add_variable("baro.aslRaw", "float") self._cf.log.add_config(self._log) if self._log.valid: self._log.data_received_cb.add_callback(self._log_data) self._log.error_cb.add_callback(self._log_error) self._log.start() else: print("Could not add logconfig -- TOC?") Thread(target=self._hover_loop).start() def _log_data(self, timestamp, data, logconf): self._asl = data["baro.aslRaw"] # print "[%d][%s]: %s" % (timestamp, logconf.name, self._asl) def _log_error(self, logconf, msg): 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) 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 _vision_loop(self): while not self._exit: self._vision_cycle(show=True) def _vision_cycle(self, show=False): # log = open(self._img_log_path, 'w', 0) # while not self._exit: img = self._cam.getImage() # image adjustments if self._img_size: img = img.resize(self._img_size[0], self._img_size[1]) img = img.rotate90() # blob search color = img - img.colorDistance(self._img_color) blobs = color.findBlobs(-1, self._img_blob_min, self._img_blob_max) # blob find if blobs is not None: self._img_pos = blobs[-1] print blobs[-1] # blob show if show: if blobs is not None: roiLayer = SimpleCV.DrawingLayer((img.width, img.height)) for blob in blobs: blob.draw(layer=roiLayer) roiLayer.circle((self._img_pos.x,self._img_pos.y), 50, SimpleCV.Color.RED, 2) img.addDrawingLayer(roiLayer) blobs.draw(SimpleCV.Color.GREEN, 1) img = img.applyLayers() img.show() # dy is the desired recalibration return self._img_pos.y - self._img_set_point # log.write("{y},{dy}\n".format(y=self._img_pos.y,dy=dy)) #log.close() def _hover_loop(self): """ # aquire asl_start over first 3 seconds -- 30 samples asl_start = 0 asl_readings = 0 while not self._alt_accepted or asl_readings == 0: time.sleep(0.01) asl_start += self._asl asl_readings += 1 print "\rAltitude reading = %s (#%d, last=%s)" % (asl_start / asl_readings, asl_readings, self._asl), asl_start = asl_start / asl_readings # print out print "" print "" print "Starting altitude = %s" % asl_start asl_target = asl_start + self._hover_target print "Hover target = %s" % self._hover_target print "Target altitude = %s" % asl_target print "Flight time = %s" % self._flight_time """ # flight loop flight_time_start = time.time() control_time_last = 0 hover_thrust = 38000 corrective_thrust = 5000 dy_scale = 1.0/400.0 # 800 pixels tall while time.time() - flight_time_start < 60 and not self._exit: if time.time() - control_time_last > 0.1: # control at most every 10ms control_time_last = time.time() dy = self._vision_cycle() thrust = hover_thrust + (dy * dy_scale * corrective_thrust) print "dy=%s, thrust=%s" % (dy, thrust) # self._cf.commander.send_setpoint(0,0,0,thrust) """ asl_diff = asl_target - self._asl # lift off speed if asl_diff > 0.6: thrust = 42000 # come down speed elif asl_diff < -25: thrust = 25000 # get to hover point else: thrust = hover_thrust + asl_diff * corrective_thrust # update our concept of what hover thrust is print "%s : %s (last=%s)" % (asl_diff, thrust, self._asl) self._cf.commander.send_setpoint(0,0,0,thrust) # upwards corrections seem fine # drift down seems worse # """ print "Killing Thrust" self._cf.commander.send_setpoint(0,0,0,0) time.sleep(1) print "Stopped." self._cf.close_link() '''
class Crazy: def __init__(self,ui): # zmienne pomocnicze w obsludze GUI self.ui=ui self.thrust=0 self.pitch=0 self.roll=0 self.yaw=0 self.startup=True self.is_connected=False # ustawienia biblioteki cf self.cf=Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) #funkcja inicjujaca polaczenie def connect(self,uri): self.cf.open_link(uri) self.is_connected=True # funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie def connected(self,uri): self.ui.l_conn.setText("Connected to {}".format(uri)) print("Connected to {}".format(uri)) control=Thread(target=self.send_ctrl) control.start() # self.log_thread() # funkcja wywolywana w chwili zakonczenia transmisji def disconnected(self,uri): self.ui.l_conn.setText("Disconnected") print("disconnected from {}".format(uri)) self.is_connected=False # funkcja konczaca polaczenie def close(self): self.cf.close_link() # ustawienia watku zbierajacego dane def log_thread(self): self.log = LogConfig(name="logs", period_in_ms=100) self.log.add_variable("stabilizer.roll", "float") self.log.add_variable("stabilizer.pitch", "float") self.log.add_variable("stabilizer.yaw", "float") try: self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." # odbieranie danych def log_received(self,timestamp, data, logconf): self.katy[0]=data["stabilizer.roll"] self.katy[1]=data["stabilizer.pitch"] self.katy[2]=data["stabilizer.yaw"] self.ui.l_pitch_a.setText("Pitch: {:.3f}".format(self.katy[1])) self.ui.l_roll_a.setText("Roll: {:.3f}".format(self.katy[0])) self.ui.l_yaw_a.setText("Yaw: {:.3f}".format(self.katy[2])) # print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"])) def log_error(self,logconf, msg): print("error while loggoing {}\n{}".format(logconf.name,msg)) # zmiana ustawien sterowania def update_ctrl(self,thrust,pitch,roll,yaw): print("thrust changed to {}".format(thrust)) print("yaw changed to {}".format(yaw)) print("pitch changed to {}".format(pitch)) print("roll changed to {}".format(roll)) self.thrust=thrust self.pitch=pitch self.roll=roll self.yaw=yaw # watek wysylajacy sterowanie def send_ctrl(self): while True: if self.thrust > 60000: self.thrust=60000 if self.thrust < 0: self.thrust=0 #Unlock startup thrust protection if self.startup: self.cf.commander.send_setpoint(0,0,0,0) self.startup=False self.cf.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) sleep(0.01)
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=1) #self._pubTemp = rospy.Publisher('temperature', Temperature, queue_size=1) #self._pubMag = rospy.Publisher('magnetic_field', MagneticField, queue_size=1) #self._pubPressure = rospy.Publisher('pressure', Float32, queue_size=1) self._pubBattery = rospy.Publisher('battery', Float32, queue_size=1) 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._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 #print('pub battery') #print(rospy.get_rostime()) msg.data = data["pm.vbat"] self._pubBattery.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 = min(max(10000, int(self._cmdVel.linear.z)),60000) #print(roll, pitch, yawrate, thrust) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) #self._cf.commander.send_setpoint(roll, pitch, yawrate, 10000) def _cmdVelChanged(self, data): self._cmdVel = data self._send_setpoint() def _update(self): #r = rospy.Rate(100) while not rospy.is_shutdown(): #sec =rospy.get_time() if self._state == CrazyflieROS.Disconnected: self._try_to_connect() #rospy.sleep(0.2) #print(0) 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() #print(1) else: self._cmdVel = Twist() self._send_setpoint() #print(2) rospy.sleep(0.2) #rospy.sleep(0.2) else: #While is connecting, wait #rospy.sleep(0.2) rospy.sleep(0.5) #sec = 1 #rospy.sleep(0.03) #r.sleep() #rospy.spin() #print(rospy.get_time()-sec) 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) #print(4) 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.""" 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") self._lg_stab.add_variable("stabilizer.thrust", "float") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).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) 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 = 45000 thrust_step = 250 self._cf.param.set_value("flightmode.althold", "False") self._cf.commander.send_setpoint(0,0,0,0) totalTime = 0 self._cf.commander.send_setpoint(0, 0, 0, 60000) time.sleep (.5) self._cf.commander.send_setpoint(0, 0, 0, 60000) self._cf.param.set_value("flightmode.althold", "True") #time.sleep (.5) while totalTime <= 1: self._cf.commander.send_setpoint(0, -1, 0, 44500) time.sleep (.1) totalTime += .01 #self._cf.param.set_value("flightmode.althold", "False") time.sleep (.5) while thrust >=5000: thrust -= thrust_step self._cf.commander.send_setpoint(0, 0, 0, thrust) time.sleep (.01) self._cf.commander.send_setpoint(0, 0, 0, 0) print "done" self._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 """ # 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) # The definition of the logconfig can be made before connecting 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("acc.zw", "float") self._lg_stab.add_variable("rangefinder.range") # 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.") # 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)) 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 get_stabilizer_log_config(self): stab_log_conf = LogConfig("stabilizer", period_in_ms=100) stab_log_conf.add_variable("stabilizer.roll", "float") stab_log_conf.add_variable("stabilizer.pitch", "float") stab_log_conf.add_variable("stabilizer.yaw", "float") return stab_log_conf
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()
def get_gyroscope_log_config(self): log_conf = LogConfig("GyroScope", period_in_ms=100) log_conf.add_variable("gyro.x", "float") log_conf.add_variable("gyro.y", "float") log_conf.add_variable("gyro.z", "float") return log_conf
class Logging: # #Logging class that logs the Stabilizer from a supplied #link uri. # 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 # The definition of the logconfig can be made before connecting 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") self._lg_stab.add_variable("acc.x", "float") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since stab variables are not in TOC") 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): global data_stabilizer data_stabilizer = (data["stabilizer.roll"], data["stabilizer.pitch"],data["stabilizer.yaw"]) 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 Crazy: def __init__(self, l_gyro, l_acc, l_status): # zmienne pomocnicze w obsludze GUI self.conn = l_status self.l_gyro = l_gyro self.l_acc = l_acc self.is_connected = False self.r = 0 self.p = 0 self.y = 0 # ustawienia biblioteki cf self.cf = Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) #funkcja inicjujaca polaczenie def connect(self, uri): self.cf.open_link(uri) self.is_connected = True # funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie def connected(self, uri): self.conn.setText("Connected to {}".format(uri)) print("Connected to {}".format(uri)) self.log_thread() Thread(target=self.send_ctrl).start() # funkcja wywolywana w chwili zakonczenia transmisji def disconnected(self, uri): self.conn.setText("Disconnected") print("disconnected from {}".format(uri)) self.is_connected = False # funkcja konczaca polaczenie def close(self): self.cf.close_link() # ustawienia watku zbierajacego dane def log_thread(self): self.log_acc = LogConfig(name="acc", period_in_ms=100) self.log_acc.add_variable("acc.x", "float") self.log_acc.add_variable("acc.y", "float") self.log_acc.add_variable("acc.z", "float") self.log_acc.add_variable("gyro.x", "float") self.log_acc.add_variable("gyro.y", "float") self.log_acc.add_variable("gyro.z", "float") try: self.cf.log.add_config(self.log_acc) # This callback will receive the data self.log_acc.data_received_cb.add_callback(self.log_received_acc) # # This callback will be called on errors self.log_acc.error_cb.add_callback(self.log_error) # Start the logging self.log_acc.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." # odbieranie danych def log_received_acc(self, timestamp, data, logconf): rate = 0.1 self.r += data['gyro.x'] * rate self.p += data['gyro.y'] * rate self.y += data['gyro.z'] * rate self.l_acc[0].setText("roll: {:.3f}".format(self.r)) self.l_acc[1].setText("pitch: {:.3f}".format(self.p)) self.l_acc[2].setText("yaw: {:.3f}".format(self.y)) self.l_gyro[0].setText("x: {:.3f}".format(data["gyro.x"])) self.l_gyro[1].setText("y: {:.3f}".format(data["gyro.y"])) self.l_gyro[2].setText("z: {:.3f}".format(data["gyro.z"])) # print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"])) def log_error(self, logconf, msg): print("error while loggoing {}\n{}".format(logconf.name, msg))
class LoggingExample: """ Simple logging example class that logs the Stabilizer from a supplied link uri and disconnects after 5s. """ def __init__(self, link_uri,results, size=(600,500)): self.thrust_mult = 1 self.thrust_step = 500 self.thrust = 20000 self.pitch = 0 self.roll = 0 self.yawrate = 0 """ Initialize and run the example with the specified link_uri """ #self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #self.s.setblocking(0) #self.s.bind((TCP_IP, TCP_PORT)) # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() print "Result test: ",results # 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 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) self.app = QtGui.QApplication([]) self.interval = int(0.1*1000) self.bufsize = 60 self.databuffer = collections.deque([0,0]*self.bufsize, self.bufsize) self.x = [0.0 , 1.0]#linspace(0,1000,60) self.y = zeros(self.bufsize, dtype=float) self.plt = pg.plot(title = 'Real Time Sensor Evaluation') self.plt.resize(*size) self.plt.showGrid(x=True, y=True) #self.plt.setYRange(-20,30, padding = 0) self.curve = self.plt.plot(self.x,results,pen=(255,0,0)) self.timer = QtCore.QTimer() self.timer.timeout.connect(self.updateplot) self.timer.start(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 # The definition of the logconfig can be made before connecting #changed #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") self._lg_stab = LogConfig(name="mb", period_in_ms=10) self._lg_stab.add_variable("mb.distance", "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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") # Start a timer to disconnect in 10s t = Timer(1000000, 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 serv_listen(self): self.s.listen(1) self.conn, self.addr = self.s.accept() self.data_buff = sel f.conn.recv(BUFFER_SIZE) #print self.data_buff return self.data_buff def updateplot(self): #self.databuffer.append(results) #self.y[:] = self.databuffer #self.y.append(data) results_plot = resultspyqt[-500:] x_plot = self.x[-500:] self.plt.setYRange(-30,30, padding = 0) self.curve.setData(x_plot,results_plot) self.app.processEvents() def getdata(data): str1=data['mb.distance'] num1=float(str1) num1=30-num1 return num1 def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" #print strftime("%H:%M:%S ", gmtime()) str1=data['mb.distance'] num1=float(str1) num1=30-num1 #self.updateplot(num1) #print "test: ",num1 #self.databuffer.append(num1) #self.y[:] = self.databuffer #self.curve.setData(x,num1) #self.app.processEvents() results.append(num1) resultspyqt.append(num1) self.x = list(range(0,len(resultspyqt))) print "[%d][%s]: %s" % (timestamp, logconf.name, data) #if not data: break data=self.serv_listen() if data>0: print "app: ",data if(int(data)<100):#we are in thrust print "thrust" print self.roll, self.pitch, self.yawrate, self.thrust self.thrust=int(data)*600 self._cf.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust) #time.sleep(0.1) elif((int(data)>100)and(int(data)<200)):#we are in pitch print roll, pitch, yawrate, thrust pitch=(int(data))/5-30 self._cf.commander.send_setpoint(roll, (int(data))/5-30, yawrate, thrust) #time.sleep(0.1) elif(int(data)>200):#we are in roll print "add roll: ",150-(int(data))*3/5 print roll, pitch, yawrate, thrust roll=50-(int(data))/5 self._cf.commander.send_setpoint(50-(int(data))/5, pitch, yawrate, thrust) #time.sleep(0.1) if data == 'Hover': print "app: ",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) #print results 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 #print results def run(self): self.app.exec_()
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 = True def connected(self, uri): print("Connected to {}".format(uri)) # Thread(target=self.motor).start() self.log_thread() def disconnected(self, uri): print("disconnected from {}".format(uri)) def close(self): self.cf.close_link() def log_thread(self): self.log = LogConfig(name="acc", period_in_ms=100) self.log.add_variable("acc.x", "float") self.log.add_variable("acc.y", "float") self.log.add_variable("acc.z", "float") try: self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." t = Timer(5, self.close) def log_received(self, timestamp, data, logconf): # self.x.setText("x: {}".format["acc.x"]) # self.y.setText("y: {}".format["acc.y"]) # self.z.setText("z: {}".format["acc.z"]) print("{}, {} = {} on {}".format(timestamp, logconf.name, data["acc.x"], self.uri)) def log_error(self, logconf, msg): print("error while loggoing {}\n{}".format(logconf.name, msg)) def motor(self): thrust_mult = 1 thrust_step = 50 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) 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()
class Crazy(QObject): cf_connected = pyqtSignal(bool) cf_logs = pyqtSignal(dict) def __init__(self, parent): super(Crazy, self).__init__() # zmienne pomocnicze w obsludze GUI self.thrust = 0 self.pitch = 0 self.roll = 0 self.yaw = 0 self.startup = True self.is_connected = False self.busy = False self.control_started = False # zmienna do wysylania logow self.log_data = { "roll": 0.0, "pitch": 0.0, "yaw": 0.0, "thrust": 0.0, "m1": 0.0, "m2": 0.0, "m3": 0.0, "m4": 0.0 } # ustawienia dobierania danych do sterowania parent.control_data_sig.connect(self.update_ctrl_sig) # ustawienia biblioteki cf self.cf = Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) self.cf.connection_lost.add_callback(self.lost_connection) #ustawienie watku sterowania self.control = Thread(target=self.send_ctrl) #funkcja inicjujaca polaczenie def connect(self, uri): self.cf.open_link(uri) # funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie def connected(self, uri): self.is_connected = True print("Connected to {}".format(uri)) self.log_thread() self.cf_connected.emit(self.is_connected) self.control.start() self.control_started = True # funkcja wywolywana w chwili zakonczenia transmisji def disconnected(self, uri): print("disconnected from {}".format(uri)) self.is_connected = False self.cf_connected.emit(self.is_connected) if self.control_started: self.control.join(0.1) # funkcja wywolywana w chwili przerwania transmisji def lost_connection(self, uri, var): print("disconnected from {}".format(uri)) self.is_connected = False self.cf_connected.emit(self.is_connected) if self.control_started: self.control.join(0.1) # funkcja konczaca polaczenie def close(self): self.cf.close_link() # ustawienia watku zbierajacego dane def log_thread(self): print "test" self.log = LogConfig(name="logs", period_in_ms=10) self.log.add_variable("stabilizer.roll", "float") self.log.add_variable("stabilizer.pitch", "float") self.log.add_variable("stabilizer.yaw", "float") # self.log.add_variable("stabilizer.thrust", "float") # self.log.add_variable("motor.m1", "float") # self.log.add_variable("motor.m2", "float") # self.log.add_variable("motor.m3", "float") # self.log.add_variable("motor.m4", "float") try: print "test 2" self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." # odbieranie danych def log_received(self, timestamp, data, logconf): # print"log" try: self.log_data["roll"] = data["stabilizer.roll"] self.log_data["pitch"] = data["stabilizer.pitch"] self.log_data["yaw"] = data["stabilizer.yaw"] # self.log_data["thrust"]=data["stabilizer.thrust"] # self.log_data["m1"]=data["motor.m1"] # self.log_data["m2"]=data["motor.m2"] # self.log_data["m3"]=data["motor.m3"] # self.log_data["m4"]=data["motor.m4"] except: print("blad logowania") self.cf_logs.emit(self.log_data) # print (self.log_data) def log_error(self, logconf, msg): ("error while loggoing {}\n{}".format(logconf.name, msg)) # zmiana ustawien sterowania - stara wersja - nieuzywana def update_ctrl(self, thrust, pitch, roll, yaw): # ("thrust | pitch | roll | yaw ") self.busy = True # ("{:.3f} | {:.3f} | {:.3f} | {:.3f}".format(thrust,pitch,roll,yaw)) self.thrust = thrust self.pitch = pitch self.roll = roll self.yaw = yaw self.busy = False # zmiana ustawien sterowania - aktualna wersja def update_ctrl_sig(self, data): # self.busy=True # print("{:.3f} | {:.3f} | {:.3f} | {:.3f}".format(data["thrust"],data["pitch"],data["roll"],data["yaw"])) self.thrust = data["thrust"] self.pitch = data["pitch"] self.roll = data["roll"] self.yaw = data["yaw"] # self.busy=False # watek wysylajacy sterowanie def send_ctrl(self): while self.is_connected: if not self.busy: if self.thrust > 60000: self.thrust = 60000 if self.thrust < 0: self.thrust = 0 #Unlock startup thrust protection if self.startup: self.cf.commander.send_setpoint(0, 0, 0, 0) self.startup = False # self.cf.commander.set_client_xmode(False) self.cf.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) sleep(0.01)
class Listener(libmyo.DeviceListener): interval = 0.05 # Output only 0.05 seconds def __init__(self): super(Listener, self).__init__() self.orientation = None self.pose = libmyo.Pose.rest self.emg_enabled = False self.locked = False self.rssi = None self.emg = None self.last_time = 0 global channel link_uri = "radio://0/" + channel + "/2M" 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.roll_p = 0 self.pitch_p = 0 self.roll_h = 0 self.pitch_h = 0 self.thrust_h = 0 self.holder = 0 self.lock = 1 self.quat = libmyo.Quaternion(0, 0, 0, 1) quat_w = 0 quat_x = 0 quat_y = 0 quat_z = 0 print("Connecting to %s" % link_uri) #MyoBand def on_connect(self, myo, timestamp, firmware_version): myo.vibrate('short') myo.vibrate('short') myo.request_rssi() myo.request_battery_level() def on_rssi(self, myo, timestamp, rssi): self.rssi = rssi def on_pose(self, myo, timestamp, pose): if pose == libmyo.Pose.double_tap: self.holder = 0 print("tap") elif pose == libmyo.Pose.fingers_spread: self._cf.commander.send_setpoint(0, 0, 0, 0) #Unlocks thrust self.lock = 0 print("finger") elif pose == libmyo.Pose.fist: self._cf.commander.send_setpoint(0, 0, 0, 0) #Clear packets time.sleep(0.1) self.lock = 1 self._cf.close_link() print("fist") self.pose = pose def on_orientation_data(self, myo, timestamp, quat): roll_w = int( math.atan2(2.0 * (quat.w * quat.x + quat.y * quat.z), 1.0 - 2.0 * (quat.x * quat.x + quat.y * quat.y)) * 100 / math.pi) thrust = float( math.asin( max(-1.0, min(1.0, 2.0 * (quat.w * quat.y - quat.z * quat.x))))) if self.holder == 0: self.thrust_h = int((thrust + math.pi / 2.0) / math.pi * 70000) self.quat = libmyo.Quaternion(quat.x, quat.y, quat.z, quat.w).conjugate() self.holder = 1 tempquat = quat * self.quat pitch_w = int( math.atan2( 2.0 * (tempquat.w * tempquat.z + tempquat.x * tempquat.y), 1.0 - 2.0 * (tempquat.y * tempquat.y + tempquat.z * tempquat.z)) * 100 / math.pi) thrust_w = abs( int((thrust + math.pi / 2.0) / math.pi * 70000) - self.thrust_h) print(pitch_w, " ", roll_w, " ", thrust_w) yaw = 0 self._cf.commander.send_setpoint(roll_w, pitch_w, yaw, thrust_w) self.orientation = quat def on_accelerometor_data(self, myo, timestamp, acceleration): pass def on_gyroscope_data(self, myo, timestamp, gyroscope): pass def on_emg_data(self, myo, timestamp, emg): self.emg = emg def on_unlock(self, myo, timestamp): self.locked = False def on_lock(self, myo, timestamp): self.locked = True def on_event(self, kind, event): pass def on_event_finished(self, kind, event): pass def on_pair(self, myo, timestamp, firmware_version): pass def on_unpair(self, myo, timestamp): pass def on_disconnect(self, myo, timestamp): pass def on_arm_sync(self, myo, timestamp, arm, x_direction, rotation, warmup_state): pass def on_arm_unsync(self, myo, timestamp): pass def on_battery_level_received(self, myo, timestamp, level): pass def on_warmup_completed(self, myo, timestamp, warmup_result): pass #CrazyFlie def _connected(self, link_uri): global connected self._cf.commander.send_setpoint(0, 0, 0, 0) connected = True self._lg_acc = LogConfig(name="Acceleration", period_in_ms=10) self._lg_acc.add_variable("acc.zw", "float") self._lg_acc.add_variable("altHold.target", "float") self._lg_acc.add_variable("stabilizer.thrust", "float") try: self._cf.log.add_config(self._lg_acc) # This callback will receive the data self._lg_acc.data_received_cb.add_callback(self._acc_log_data) # This callback will be called on errors self._lg_acc.error_cb.add_callback(self._acc_log_error) # Start the logging self._lg_acc.start() except KeyError as e: print("Could not start log configuration," "{} not found in TOC".format(str(e))) except AttributeError: print("Could not add Acceleration log config, bad configuration.") def _connection_failed(self, link_uri, msg): global connected print("Connection to %s failed: %s" % (link_uri, msg)) connected = False def _connection_lost(self, link_uri, msg): global connected print("Connection to %s lost: %s" % (link_uri, msg)) connected = False def _disconnected(self, link_uri): global connected print("Disconnected from %s" % link_uri) connected = False def _acc_log_error(self, logconf, msg): print("Error when logging %s: %s" % (logconf.name, msg)) def _acc_log_data(self, timestamp, data, logconf): """if(data["acc.zw"] < -0.98):
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 """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie(ro_cache="./crazyflie-clients-python/lib/cflib/cache", rw_cache="./crazyflie-clients-python/lib/cflib/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) 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 # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=2000) self._lg_stab.add_variable("gyro.x", "float") self._lg_stab.add_variable("gyro.y", "float") self._lg_stab.add_variable("gyro.z", "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") self._lg_stab.add_variable("stabilizer.thrust", "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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") self._cf.commander.send_setpoint(0, 0, 0, 20000) 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) new_dict = {logconf.name:data} x = new_dict['Stabilizer']['stabilizer.roll'] y = new_dict['Stabilizer']['stabilizer.pitch'] z = new_dict['Stabilizer']['stabilizer.yaw'] print "%s" % y 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 Quad: """ 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 """ # 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 Thread(target=self._control).start() Thread(target=self._input).start() ######################################################################## # Set up Stabilizer Logger ######################################################################## self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50) self._lg_stab.add_variable("stabilizer.roll", "float") self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._log_stab_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._log_error) # Start the logging self._lg_stab.start() else: print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Battery Logger # ######################################################################## self._lg_bat = LogConfig(name="Battery",period_in_ms=50) self._lg_bat.add_variable("pm.vbat", "float") self._cf.log.add_config(self._lg_bat) if self._lg_bat.valid: # This callback will receive the data self._lg_bat.data_received_cb.add_callback(self._log_bat_data) # This callback will be called on errors self._lg_bat.error_cb.add_callback(self._log_error) # Start the logging self._lg_bat.start() else: print("Could not setup loggingblock! You dun goofed") # ######################################################################## # # Set up Acc Logger # ######################################################################## # self._lg_acc = LogConfig(name="Acc", period_in_ms=50) # self._lg_acc.add_variable("acc.x", "float") # self._lg_acc.add_variable("acc.y", "float") # self._lg_acc.add_variable("acc.z", "float") # self._cf.log.add_config(self._lg_acc) # if self._lg_acc.valid: # # This callback will receive the data # self._lg_acc.data_received_cb.add_callback(self._log_acc_data) # # This callback will be called on errors # self._lg_acc.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_acc.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Gyro Logger # ######################################################################## # self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50) # self._lg_gyro.add_variable("gyro.x", "float") # self._lg_gyro.add_variable("gyro.y", "float") # self._lg_gyro.add_variable("gyro.z", "float") # self._cf.log.add_config(self._lg_gyro) # if self._lg_gyro.valid: # # This callback will receive the data # self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data) # # This callback will be called on errors # self._lg_gyro.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_gyro.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Baro Logger # ######################################################################## self._lg_baro = LogConfig(name="Baro", period_in_ms=50) self._lg_baro.add_variable("baro.aslLong", "float") self._cf.log.add_config(self._lg_baro) if self._lg_baro.valid: # This callback will receive the data self._lg_baro.data_received_cb.add_callback(self._log_baro_data) # This callback will be called on errors self._lg_baro.error_cb.add_callback(self._log_error) # Start the logging self._lg_baro.start() else: print("Could not add logconfig since some variables are not in TOC") 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 _log_stab_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_roll, g_yaw, g_pitch, g_thrust (g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"]) # def _log_acc_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_acc_x, g_acc_y, g_acc_z; # (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"]) # def _log_gyro_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_gyro_x, g_gyro_y, g_gyro_z; # (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"]) def _log_baro_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_baro; g_baro = data["baro.aslLong"] def _log_bat_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_bat; g_bat = data["pm.vbat"] def _control(self): global g_roll, g_yaw, g_pitch, g_thrust global g_gyro_x, g_gyro_y, g_gyro_z global g_acc_x, g_acc_y,g_acc_z global g_baro, g_init_baro, g_kill global diff_baro global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax global count global pause global g_bat print "Control Started" while not g_kill: if (count%100==0): #print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z), #print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z), print "thrust: %d" % thrust, print "battery: %10f" % g_bat, print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro) print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch) count = count + 1 if not (g_init_baro == 0): diff_baro = g_init_baro - g_baro roll = 0 pitch = 0 yawrate = 0 if pause == 's': thrust = thrustInit elif pause == 'l': thrust = 0 self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.01) # thrust_mult = 1 # thrust_step = 500 # thrust = 20000 # pitch = 0 # roll = 0 # yawrate = 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 self._cf.close_link() def _input(self): global g_baro, g_init_baro, g_kill, pause print "Input Started" while g_kill is False: command = raw_input("Input Command:\n") if (command[0] == 'S') or (command[0] == 's'): g_init_baro = g_baro-1 pause = 's' elif (command[0] == 'l'): pause = 'l' elif (command[0] == 'K') or (command[0] == 'k'): self._cf.commander.send_setpoint(0, 0, 0, 0) g_kill = True print "KILLED!!!" 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 LogData: def __init__(self, link_uri): self.thrust = 10001 self.roll = self.pitch = self.yaw = 0 #Sensors being monitored self.r = self.p = self.y = 0 self.ax = self.ay = self.az = 0 self.baro = 0 # Create a Crazyflie object without specifying any cache dirs self.crazyflie = Crazyflie() # The definition of the logconfig self._lg_stab = LogConfig(name="Logging", 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") self._lg_stab.add_variable("acc.x", "float") self._lg_stab.add_variable("acc.y", "float") self._lg_stab.add_variable("acc.z", "float") #self._lg_stab.add_variable("baro.aslLong", "float") # Connect some callbacks from the Crazyflie API 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) print("Connecting to %s" % link_uri) # Try to connect to the Crazyflie self.crazyflie.open_link(link_uri) # Variable used to keep main loop occupied until disconnect self.is_connected = False def _connected(self, link_uri): print("Connected to %s" % link_uri) self.crazyflie.is_connected = True #create a thread that published the set points #Thread(target=self.pulse_command).start() # Adding the configuration try: #self.crazyflie.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() print "yay" Thread(target=self.pulse_command).start() print "Hello I am out of " 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.") # Start a timer to disconnect in 10s #t = Timer(5, self.crazyflie.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 _accel_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 from the log API when data arrives""" self.r = data["stabilizer.roll"] self.p = data["stabilizer.pitch"] self.y = data["stabilizer.yaw"] self.ax = data["acc.x"] self.ay = data["acc.y"] self.az = data["acc.z"] #self.baro = data["baro.aslLong"] print self.ax 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 pulse_command(self): print 'in pulse_command' self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) #self.crazyflie.commander.send_setpoint(0,0,0,20000) time.sleep(0.1)
class logs: def __init__(self, cf): #self.log_file_next_stuff = open("log_file_next.txt", "w+") #self.log_file_stab = open("log_file_stab.txt", "w+") #local copy of crazy_Auto self._cf = cf # Roll, Pitch, Yaw self.init_state = [0,0,0] self.state = [0,0,0] self.next_state = [0,0] # X, Y, Z, Mag self.init_accel_state = [0,0,0,0] self.accel_state = [0,0,0,0] # X, Y, Z self.init_gyro_state = [0,0,0] self.gyro_state = [0,0,0] # ground average, current self.altitude = [0,0] #current battery voltage self.battery_state = 0 #[roll,pitch,yaw,altitude,acc.x,acc.y,acc.z,acc.mag,gyro.x, gyro.y, gyro.z, batteryV] self.measurements = [0 for i in range(12)] self.logging_count = 0 #Setup logs once connection to crazyflie has been established self._cf._cf.connected.add_callback(self._init_flight_var) # specifying the size of the state and the observation space #self.transition_matrices = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) #self.transition_matrices.resize(4,1) #self.kf = KalmanFilter(transition_matrices=self.transition_matrices, n_dim_obs= 4) #transition_matrices=self.transition_matrices, #self._cf.connected.add_callback(self._init_flight_var) #fill in base parameters for flight def _init_flight_var(self, link_uri): print ("Connected to %s" % link_uri) self.RPY_log = LogConfig(name="Stabilizer", period_in_ms=10) self.RPY_log.add_variable("stabilizer.roll", "float") self.RPY_log.add_variable("stabilizer.pitch", "float") self.RPY_log.add_variable("stabilizer.yaw", "int16_t") self.RPY_log.add_variable("baro.asl", "float") #barometer above sea level self.RPY_log.add_variable("gyro.x", "float") self.RPY_log.add_variable("gyro.y", "float") self.RPY_log.add_variable("gyro.z", "float") self.battery_log = LogConfig(name="Battery", period_in_ms=1000) self.battery_log.add_variable("pm.vbat", "float") self.acc_log = LogConfig(name="Acc", period_in_ms = 10) self.acc_log.add_variable("acc.x", "float") self.acc_log.add_variable("acc.y", "float") self.acc_log.add_variable("acc.z", "float") self._cf._cf.log.add_config(self.RPY_log) #add the log to the CrazyFlie self._cf._cf.log.add_config(self.battery_log) self._cf._cf.log.add_config(self.acc_log) self.RPY_log.data_received_cb.add_callback(self.update_flight_params) self.RPY_log.error_cb.add_callback(self.update_error) self.battery_log.data_received_cb.add_callback(self.update_battery) self.battery_log.error_cb.add_callback(self.update_error) self.acc_log.data_received_cb.add_callback(self.update_acc) self.acc_log.error_cb.add_callback(self.update_error) self.RPY_log.start() self.battery_log.start() self.acc_log.start() print ("Logging Started\n") self._cf._cf.connected.add_callback(self._cf._connected) def update_error(self, logconf, msg): print ("Error when logging %s: %s" % (logconf.name, msg)) def update_battery(self, timestamp, data, logconf): self.battery_state = data["pm.vbat"] def update_acc(self, timestamp, data, logconf): if not self._cf.is_flying: self.init_accel_state[0] = data["acc.x"] self.init_accel_state[1] = data["acc.y"] self.init_accel_state[2] = data["acc.z"] self.init_accel_state[3] = data["acc.mag2"] return self.accel_state[0] = data["acc.x"] self.accel_state[1] = data["acc.y"] self.accel_state[2] = data["acc.z"] self.accel_state[3] = data["acc.mag2"] def update_flight_params(self, timestamp, data, logconf): #print data #print self._cf.is_flying if not self._cf.is_flying: self.init_state[0] = float(data["stabilizer.roll"]) self.init_state[1] = float(data["stabilizer.pitch"]) self.init_state[2] = data["stabilizer.yaw"] #self.init_accel_state[0] = data["acc.x"] #self.init_accel_state[1] = data["acc.y"] #self.init_accel_state[2] = data["acc.z"] #self.init_accel_state[3] = data["acc.mag2"] self.init_gyro_state[0] = data["gyro.x"] self.init_gyro_state[1] = data["gyro.y"] self.init_gyro_state[2] = data["gyro.z"] #self.battery_state = data["pm.vbat"] #if self.logging_count is 0: self.altitude[0] = data["baro.asl"] # self.logging_count += 1 #else: # self.altitude[0] = float((data["baro.asl"] + self.logging_count*self.altitude[0])/(self.logging_count+1)) #take the running average of the inital altitude for better ground reading # self.logging_count += 1 #self._cf.altitude[1] = self.altitude[0] #<-----------this shouldnt be here.... #[roll,pitch,yaw,altitude,acc.x,acc.y,acc.z,acc.mag,gyro.x, gyro.y, gyro.z, batteryV] #print self.altitude return self.state[0] = data["stabilizer.roll"] self.state[1] = data["stabilizer.pitch"] self.state[2] = data["stabilizer.yaw"] #self.accel_state[0] = data["acc.x"] #self.accel_state[1] = data["acc.y"] #self.accel_state[2] = data["acc.z"] #self.accel_state[3] = data["acc.mag2"] self.gyro_state[0] = data["gyro.x"] self.gyro_state[1] = data["gyro.y"] self.gyro_state[2] = data["gyro.z"] #self.battery_state = data["pm.vbat"] self.altitude[1] = data["baro.asl"] #print self.altitude self.next_state[0] = self.state[0] - self.init_state[0] self.next_state[1] = self.state[1] - self.init_state[0] def log_file_print(self, file, data): for i in range(len(data)): file.write(str(data[i])) file.write(',') file.write('\n') def get_measurements(self): pass #return self.measurements #self.kf = self.kf.em(self.measurements, n_iter=1) #return self.kf.filter(self.measurements) def get_gyro(self, value): if (value == 0): print ("X value: ", self.gyro_state[0]) elif(value == 1): print ("Y value: ", self.gyro_state[1]) elif(value == 2): print ("Z value: ", self.gyro_state[2]) else: return 0 def get_altitude(self): #print "altitude: ", self.baro_sensor[0] return self.altitude[0] def get_roll(self): print ("roll: ", self.state[0]) def get_pitch(self): print ("pitch: ", self.state[1]) def get_yaw(self): print ("yaw: ", self.state[2]) def get_baro(self, value): if (value == 0): print ("lRaw: ", self.baro_sensor[0]) elif(value == 1): print ("lLong: ", self.baro_sensor[0]) elif(value == 2): print ("Pressure: ", self.baro_sensor[0]) else: return 0
class Crazy: def __init__(self, ui): # zmienne pomocnicze w obsludze GUI self.ui = ui self.thrust = 0 self.pitch = 0 self.roll = 0 self.yaw = 0 self.startup = True self.is_connected = False # ustawienia biblioteki cf self.cf = Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) #funkcja inicjujaca polaczenie def connect(self, uri): self.cf.open_link(uri) self.is_connected = True # funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie def connected(self, uri): self.ui.l_conn.setText("Connected to {}".format(uri)) print("Connected to {}".format(uri)) control = Thread(target=self.send_ctrl) control.start() # self.log_thread() # funkcja wywolywana w chwili zakonczenia transmisji def disconnected(self, uri): self.ui.l_conn.setText("Disconnected") print("disconnected from {}".format(uri)) self.is_connected = False # funkcja konczaca polaczenie def close(self): self.cf.close_link() # ustawienia watku zbierajacego dane def log_thread(self): self.log = LogConfig(name="logs", period_in_ms=100) self.log.add_variable("stabilizer.roll", "float") self.log.add_variable("stabilizer.pitch", "float") self.log.add_variable("stabilizer.yaw", "float") try: self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." # odbieranie danych def log_received(self, timestamp, data, logconf): self.katy[0] = data["stabilizer.roll"] self.katy[1] = data["stabilizer.pitch"] self.katy[2] = data["stabilizer.yaw"] self.ui.l_pitch_a.setText("Pitch: {:.3f}".format(self.katy[1])) self.ui.l_roll_a.setText("Roll: {:.3f}".format(self.katy[0])) self.ui.l_yaw_a.setText("Yaw: {:.3f}".format(self.katy[2])) # print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"])) def log_error(self, logconf, msg): print("error while loggoing {}\n{}".format(logconf.name, msg)) # zmiana ustawien sterowania def update_ctrl(self, thrust, pitch, roll, yaw): print("thrust changed to {}".format(thrust)) print("yaw changed to {}".format(yaw)) print("pitch changed to {}".format(pitch)) print("roll changed to {}".format(roll)) self.thrust = thrust self.pitch = pitch self.roll = roll self.yaw = yaw # watek wysylajacy sterowanie def send_ctrl(self): while True: if self.thrust > 60000: self.thrust = 60000 if self.thrust < 0: self.thrust = 0 #Unlock startup thrust protection if self.startup: self.cf.commander.send_setpoint(0, 0, 0, 0) self.startup = False self.cf.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust) sleep(0.01)
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=True def connected(self,uri): print("Connected to {}".format(uri)) # Thread(target=self.motor).start() self.log_thread() def disconnected(self,uri): print("disconnected from {}".format(uri)) def close(self): self.cf.close_link() def log_thread(self): self.log = LogConfig(name="acc", period_in_ms=100) self.log.add_variable("acc.x", "float") self.log.add_variable("acc.y", "float") self.log.add_variable("acc.z", "float") try: self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." t=Timer(5,self.close) def log_received(self,timestamp, data, logconf): # self.x.setText("x: {}".format["acc.x"]) # self.y.setText("y: {}".format["acc.y"]) # self.z.setText("z: {}".format["acc.z"]) print("{}, {} = {} on {}".format(timestamp,logconf.name,data["acc.x"],self.uri)) def log_error(self,logconf, msg): print("error while loggoing {}\n{}".format(logconf.name,msg)) def motor(self): thrust_mult = 1 thrust_step = 50 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) 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()
class Quad: """ 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 """ # 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 Thread(target=self._control).start() Thread(target=self._input).start() ######################################################################## # Set up Stabilizer Logger ######################################################################## self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50) self._lg_stab.add_variable("stabilizer.roll", "float") self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._log_stab_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._log_error) # Start the logging self._lg_stab.start() else: print( "Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Acc Logger # ######################################################################## # self._lg_acc = LogConfig(name="Acc", period_in_ms=50) # self._lg_acc.add_variable("acc.x", "float") # self._lg_acc.add_variable("acc.y", "float") # self._lg_acc.add_variable("acc.z", "float") # self._cf.log.add_config(self._lg_acc) # if self._lg_acc.valid: # # This callback will receive the data # self._lg_acc.data_received_cb.add_callback(self._log_acc_data) # # This callback will be called on errors # self._lg_acc.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_acc.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Gyro Logger # ######################################################################## # self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50) # self._lg_gyro.add_variable("gyro.x", "float") # self._lg_gyro.add_variable("gyro.y", "float") # self._lg_gyro.add_variable("gyro.z", "float") # self._cf.log.add_config(self._lg_gyro) # if self._lg_gyro.valid: # # This callback will receive the data # self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data) # # This callback will be called on errors # self._lg_gyro.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_gyro.start() # else: # print("Could not add logconfig since some variables are not in TOC") # ######################################################################## # # Set up Baro Logger # ######################################################################## self._lg_baro = LogConfig(name="Baro", period_in_ms=50) self._lg_baro.add_variable("baro.aslLong", "float") self._cf.log.add_config(self._lg_baro) if self._lg_baro.valid: # This callback will receive the data self._lg_baro.data_received_cb.add_callback(self._log_baro_data) # This callback will be called on errors self._lg_baro.error_cb.add_callback(self._log_error) # Start the logging self._lg_baro.start() else: print( "Could not add logconfig since some variables are not in TOC") 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 _log_stab_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_roll, g_yaw, g_pitch, g_thrust (g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"]) # def _log_acc_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_acc_x, g_acc_y, g_acc_z; # (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"]) # def _log_gyro_data(self, timestamp, data, logconf): # """Callback froma the log API when data arrives""" # global g_gyro_x, g_gyro_y, g_gyro_z; # (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"]) def _log_baro_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" global g_baro g_baro = data["baro.aslLong"] def _control(self): global g_roll, g_yaw, g_pitch, g_thrust global g_gyro_x, g_gyro_y, g_gyro_z global g_acc_x, g_acc_y, g_acc_z global g_baro, g_init_baro, g_kill global diff_baro global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax global count print "Control Started" while not g_kill: #print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z), #print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z), print "thrust: %d" % thrust, print "difference: %10f" % diff_baro, print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro), print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch) if not (g_init_baro == 0): diff_baro = g_init_baro - g_baro roll = 0 pitch = -1.5 yawrate = 0 count = count + 1 if diff_baro > 0.0: thrust = thrustInit + scaleUp * diff_baro else: thrust = thrustInit + scaleDown * diff_baro if thrust < thrustMin: thrust = thrustMin if thrust > thrustMax: thrust = thrustMax self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.01) # thrust_mult = 1 # thrust_step = 500 # thrust = 20000 # pitch = 0 # roll = 0 # yawrate = 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 self._cf.close_link() def _input(self): global g_baro, g_init_baro, g_kill print "Input Started" while g_kill is False: command = raw_input("Input Command:\n") if (command[0] == 'S') or (command[0] == 's'): g_init_baro = g_baro - 1 elif (command[0] == 'K') or (command[0] == 'k'): self._cf.commander.send_setpoint(0, 0, 0, 0) g_kill = True print "KILLED!!!" 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 TestFlight: def __init__(self): """ Initialize the quadcopter """ self.crazyflie = cflib.crazyflie.Crazyflie() print 'Initializing drivers' cflib.crtp.init_drivers() print 'Searching for available devices' available = cflib.crtp.scan_interfaces() radio = False for i in available: # Connect to the first device of the type 'radio' if 'radio' in i[0]: radio = True dev = i[0] print 'Connecting to interface with URI [{0}] and name {1}'.format(i[0], i[1]) self.crazyflie.open_link(dev) break if not radio: print 'No quadcopter detected. Try to connect again.' exit(-1) # Set up the callback when connected self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): """ Set the loggers """ # Log stabilizer self.logStab = LogConfig("Stabalizer", 200) self.logStab.add_variable("stabilizer.roll", "float") self.logStab.add_variable("stabilizer.pitch", "float") self.logStab.add_variable("stabilizer.yaw", "float") self.logStab.add_variable("stabilizer.thrust", "uint16_t") self.crazyflie.log.add_config(self.logStab) if self.logStab.valid: self.logStab.data_received_cb.add_callback(self.print_stab_data) self.logStab.start() else: print 'Could not setup log configuration for stabilizer after connection!' # Log barometer self.logBaro = LogConfig("Baro", 200) self.logBaro.add_variable("baro.aslLong", "float") self.crazyflie.log.add_config(self.logBaro) if self.logBaro.valid: self.logBaro.data_received_cb.add_callback(self.print_baro_data) self.logBaro.start() else: print 'Could not setup log configuration for barometer after connection!' # Log Accelerometer self.logAccel = LogConfig("Accel",200) self.logAccel.add_variable("acc.x", "float") self.logAccel.add_variable("acc.y", "float") self.logAccel.add_variable("acc.z", "float") self.crazyflie.log.add_config(self.logAccel) if self.logAccel.valid: self.logAccel.data_received_cb.add_callback(self.print_accel_data) self.logAccel.start() else: print 'Could not setup log configuration for accelerometer after connection!' Thread(target=self.increasing_step).start() def print_baro_data(self, ident, data, logconfig): #logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"])) pass def print_stab_data(self, ident, data, logconfig): sys.stdout.write('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"])) def print_accel_data(self, ident, data, logconfig): #logging.info("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}".format(ident, data["acc.x"], data["acc.y"], data["acc.z"])) pass def increasing_step(self): global key start_thrust = 10000 # (blades start to rotate after 10000) min_thrust = 10000 max_thrust = 80000 thrust_increment = 500 start_roll = 0 roll_increment = 30 min_roll = -50 max_roll = 50 start_roll = 0 roll_increment = 30 min_roll = -50 max_roll = 50 start_pitch = 0 pitch_increment = 30 min_pitch = -50 max_pitch = 50 start_yaw = 0 yaw_increment = 30 min_yaw = -200 max_yaw = 200 stop_moving_count = 0 pitch = start_pitch roll = start_roll thrust = start_thrust yaw = start_yaw # Start the keyread thread keyreader = KeyReaderThread() keyreader.start() sys.stdout.write('\r\nCrazyflie Status\r\n') sys.stdout.write('================\r\n') sys.stdout.write("Use 'w' and 's' for the thrust, 'a' and 'd' for yaw, 'i' and 'k' for pitch and 'j' and 'l' for roll. Stop flying with 'q'. Exit with 'e'.\r\n") while key != "e": if key == 'q': thrust = 0 pitch = 0 roll = 0 yaw = 0 elif key == 'w' and (thrust + thrust_increment <= max_thrust): thrust += thrust_increment elif key == 's' and (thrust - thrust_increment >= min_roll): thrust -= thrust_increment elif key == 'd' and (yaw + yaw_increment <= max_yaw): yaw += yaw_increment stop_moving_count = 0 elif key == 'a' and (yaw - yaw_increment >= min_yaw): yaw -= yaw_increment stop_moving_count = 0 elif key == 'l' and (roll + roll_increment <= max_roll): roll += roll_increment stop_moving_count = 0 elif key == 'j' and (roll - roll_increment >= min_roll): roll -= roll_increment stop_moving_count = 0 elif key == 'i' and (pitch + pitch_increment <= max_pitch): pitch += pitch_increment stop_moving_count = 0 elif key == 'k' and (pitch - pitch_increment >= min_pitch): pitch -= pitch_increment stop_moving_count = 0 elif key == "": # The controls are not being touch, get back to zero roll, pitch and yaw if stop_moving_count >= 40: pitch = 0 roll = 0 yaw = 0 else: stop_moving_count += 1 else: pass key = "" self.crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust) 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.crazyflie.close_link()
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("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 s print("Motors ON, starting timer") t = Timer(3, 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() 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 = 400 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 >= 10000: self._cf.commander.send_setpoint(0, 0, 0, thrust) thrust += thrust_step * thrust_mult time.sleep(0.1) print ("Landing done") time.sleep(2) self._cf.close_link() def _ramp_motors(self): thrust = 15000 global landing landing = False #print(ref_baro) # 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.5) if landing == False: #self._cf.param.set_value("flightmode.althold","True") print "Hover ON" while landing == False: #thrust = 32767 self._cf.commander.send_setpoint(0,0,0,32767) self._cf.param.set_value("flightmode.althold","True") #time.sleep(0.1) print ("Hovering") time.sleep(0.1) print "Hover OFF" self._cf.param.set_value("flightmode.althold","False")
class Crazy: def __init__(self,x,y,z,conn_status): # zmienne pomocnicze w obsludze GUI self.conn=conn_status self.x=x self.y=y self.z=z self.thrust=0 self.startup=True self.is_connected=False # ustawienia biblioteki cf self.cf=Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) #funkcja inicjujaca polaczenie def connect(self,uri): self.cf.open_link(uri) self.is_connected=True # funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie def connected(self,uri): self.conn.setText("Connected to {}".format(uri)) print("Connected to {}".format(uri)) self.log_thread() Thread(target=self.send_ctrl).start() # funkcja wywolywana w chwili zakonczenia transmisji def disconnected(self,uri): self.conn.setText("Disconnected") print("disconnected from {}".format(uri)) self.is_connected=False # funkcja konczaca polaczenie def close(self): self.cf.close_link() # ustawienia watku zbierajacego dane def log_thread(self): self.log = LogConfig(name="acc", period_in_ms=100) self.log.add_variable("acc.x", "float") self.log.add_variable("acc.y", "float") self.log.add_variable("acc.z", "float") try: self.cf.log.add_config(self.log) # This callback will receive the data self.log.data_received_cb.add_callback(self.log_received) # # This callback will be called on errors self.log.error_cb.add_callback(self.log_error) # Start the logging self.log.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." # odbieranie danych def log_received(self,timestamp, data, logconf): # self.x.setText("x: {:.3f}".format(data["acc.x"])) self.y.setText("y: {:.3f}".format(data["acc.y"])) self.z.setText("z: {:.3f}".format(data["acc.z"])) # print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"])) def log_error(self,logconf, msg): print("error while loggoing {}\n{}".format(logconf.name,msg)) # zmiana ustawien sterowania def update_ctrl(self,thrust): print("thrust changed to {}".format(thrust)) self.thrust=thrust # watek wysylajacy sterowanie def send_ctrl(self): print("ctrl+send") pitch = 0 roll = 0 yawrate = 0 while True: thrust=self.thrust if thrust > 60000: thrust=60000 if thrust < 0: thrust=0 #Unlock startup thrust protection if self.startup: self.cf.commander.send_setpoint(0,0,0,0) self.startup=False self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust) sleep(0.01)
class Hover: """ This will both hover the CF and log/print data concerning the roll,pitch,yaw, etc. There are 3 separate PID functions to control: pitch, yaw, and roll. Eventually there will be a 4th to control height (or directly: thrust) """ def __init__(self, link_uri): """ The link_uri is the 'radio address' of the crazyflie """ #All of the global variables needed for the PID functions self.rollsetpoint=0.0 self.pitchsetpoint=0.0 self.yawsetpoint=0.0 self.altitudesetpoint= 20.0 self.thrust=20000 #Calculated errors self.iroll=0.0 self.droll=0.0 self.ipitch=0.0 self.dpitch=0.0 self.iyaw=0.0 self.dyaw=0.0 self.ialtitude=0.0 self.daltitude=0.0 #The last recorded timestamp (used for dt determination) self.lastTime=0 self.lastRollError=0.0 self.lastPitchError=0.0 self.lastYawError=0.0 self.lastAltitudeError=0.0 #PID coefficients self.kp_roll=0.#1.5#1.75 self.kp_pitch=0.#0.75#2.3#2.5 self.kp_yaw=0.0 self.kp_altitude=0. self.ki_yaw=0.0 self.ki_roll=0.0 self.ki_pitch=0.0#0.000005#0.00000012 self.ki_altitude=0.0 self.kd_roll=0.0 self.kd_pitch=0.0 self.kd_altitude=0.0 self.lastVal=7.0 # Create a Crazyflie object 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 # 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 # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name="HoverLog", period_in_ms=10) self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.roll","float") self._lg_stab.add_variable("stabilizer.yaw", "float") self._lg_stab.add_variable("stabilizer.thrust", "float") # self._lg_stab.add_variable("gyro.x","float") # self._lg_stab.add_variable("gyro.y","float") #self._lg_stab.add_variable("gyro.z","float") #self._lg_stab.add_variable("baro.aslLong","float") #self._lg_stab = LogConfig(name="mb", period_in_ms=10) self._lg_stab.add_variable("mb.distance","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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) # Start a timer to disconnect the crazyflie in 25 seconds t = Timer(25,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) #The following is all of the PID functions. #They have been adopted from the general form given in: #"http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/" """ def pid_roll(self,dataroll,dt): #Calculate the errors for p,i, and d error=self.rollsetpoint-dataroll self.iroll+=(error*dt) self.droll=(error-self.lastRollError)/dt #setup for next time self.lastRollError=error #return the pid value return self.kp_roll*error+self.ki_roll*self.iroll+self.kd_roll*self.droll def pid_pitch(self,datapitch,dt): #Calculate the errors for p,i, and d error=self.pitchsetpoint-datapitch self.ipitch+=(error*dt) self.dpitch=(error-self.lastPitchError)/dt #set up the last error for the next iteration self.lastPitchError=error #return the pid value return (self.kp_pitch*error+self.ki_pitch*self.ipitch+self.kd_pitch*self.dpitch) def pid_yaw(self,datayaw,dt): #Calculate the errors for p,i, and d error=self.yawsetpoint-datayaw self.iyaw+=(error*dt) self.dyaw=(error-self.lastYawError)/dt #set up the last error for the next iteration self.lastYawError=error #return the pid value return self.kp_yaw*error+self.ki_yaw*self.iyaw+0*self.dyaw #Modification to replace Barometer with Sonar Range Finder and optimize thrust using that. #def thrust_from_sonar(self,dataSonar): #Based on the "target altitude" argument sent in from the console this is a PID loop based on that setpoint # def thrust_from_barometer(self,aslLong,dt): # """old thrust formulation # base = int(sys.argv[1]) # return 15000+(base-aslLong)*800 # """ # #Calculate the errors for p,i, and d # error=self.altitudesetpoint-aslLong # print "error: ",error # self.ialtitude+=(error*dt) # self.datitude=(error-self.lastAltitudeError)/dt # #set up the last error for the next iteration # self.lastAltitudeError=error # #return the pid value # if (((self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000)>60000): # return 55000 # elif (((self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000)<20000): # return 25000 # return (self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000 def _stab_log_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" #Print the log data print "[%d][%s]: %s" % (timestamp, logconf.name, data) #print self.pid_pitch(data['stabilizer.pitch'],dt) #calculate the dt for this iteration. Should be close to 10ms now = timestamp dt = timestamp-self.lastTime print (self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000 #set the controls of the CF if (abs(self.altitudesetpoint-data['mb.distance'])>8): # and \ # (self.lastVal>(((self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000)*0.8))and \ # (self.lastVal>(((self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000)*0.8)): self.thrust=(self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000 #lif (abs(self.altitudesetpoint-data['mb.distance'])>20): #self.thrust=(self.altitudesetpoint-data['mb.distance'])*300+(self.altitudesetpoint*1000)+25000 self._cf.commander.send_setpoint(self.pid_roll(data['stabilizer.roll'],dt),-1*self.pid_pitch(data['stabilizer.pitch'],dt), self.pid_yaw(data['stabilizer.yaw'],dt),self.thrust) self.lastVal=self.thrust #set up the lastTime for the next iteration self.lastTime=now 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 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_sent = 0.0 self._pitch_sent = 0.0 self._yaw_sent = 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") exit(0) 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('./ramp_accu_log','w') self._ramp() 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(self): thrust_mult = 1 thrust_step = 100 thrust = 35000 pitch = 3.95 roll = -5 yawrate = 0 # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) while thrust >= 35000: print("roll: "+str(self._roll_list[0])) print("pitch: "+str(self._pitch_list[0])) print("yawrate: "+str(self._yaw_list[0])) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 38000: 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() 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 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 """ # 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 rospy.init_node("angles_and_commander", anonymous = True) self.pub = rospy.Publisher('angle_sensors', angles) rospy.Subscriber("control", rpyt, self.ros_callback) self.roll_rate = 0 self.pitch_rate = 0 self.yaw_rate = 0 self.thrust = 0 self.kill = 0 self.t = 0 # Try to connect to the Crazyflie self._cf.open_link(link_uri) print "0" #Set up logging bag stri = String() #stri.data = "Alpha: " + str(rospy.get_param('ALPHA')) + " LAG: " + str(rospy.get_param('LAG')) + " VARS: " + str(rospy.get_param('VARS')) stri.data = 'test' d = datetime.datetime.now() path = '/home/cudauser/irobot_quad/quad_cat/logging/' name = str(d) + "__" + "crazyflie_angles__" + "cmds_motors" full_name = os.path.join(path, name + ".bag") self.bag = rosbag.Bag(full_name, 'w') self.bag.write('metadata', stri) print "1" # Variable used to keep main loop occupied until disconnect self.is_connected = True def ros_callback(self, cmd): self.roll_rate = cmd.roll_rate self.pitch_rate = cmd.pitch_rate self.yaw_rate = cmd.yaw_rate self.thrust = cmd.thrust self.kill = max(self.kill, cmd.kill) 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 the motors #Thread(target=self._ramp_motors).start() # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name="Sensors", 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") self._lg_stab.add_variable("pm.vbat", "float") """ self._lg_stab.add_variable("motor.m1", "float") self._lg_stab.add_variable("motor.m2", "float") self._lg_stab.add_variable("motor.m3", "float") self._lg_stab.add_variable("motor.m4", "float") print "4" """ # 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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") 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 "Entered callback _stab_log_data()" s = angles() roll = data['stabilizer.roll']*np.pi/180.0 pitch = data['stabilizer.pitch']*np.pi/180.0 yaw = data['stabilizer.yaw']*np.pi/180.0 s.roll = roll s.pitch = pitch s.yaw = yaw s.bat = data['pm.vbat'] #s.gyro_x = acceleration[0] #s.gyro_y = acceleration[1] #s.gyro_z = acceleration[2] self.pub.publish(s) self.bag.write('angles', s) #self.bag.write('battery', Float32(data['pm.vbat'])) print "Printing kill" + str(self.kill) if (not rospy.is_shutdown() and self.kill == 0 ): self.t += 1 mult = 1.0 roll_cmd = self.roll_rate*180/np.pi * 0.5 + 2 pitch_cmd = -(self.pitch_rate*180/np.pi) * 0.5 - 11 yaw_cmd = -(self.yaw_rate*180/np.pi) thrust_cmd = ((9.81 + self.thrust) - 8.778)/2.4e-05 + 2500 self._cf.commander.send_setpoint(roll_cmd, pitch_cmd, yaw_cmd, thrust_cmd) # crazy debug block #roll_cmd = 0 #pitch_cmd = 30 #yaw_cmd = 0 #thrust_cmd = 45000 #print "B" #if self.t > 2500: # mult = -1.0 #msg1 = motor_msg() #msg1.m1 = data['motor.m1'] #msg1.m2 = data['motor.m2'] #msg1.m3 = data['motor.m3'] #msg1.m4 = data['motor.m4'] #msg2 = rpyt() #msg2.roll_rate = self.roll_rate #msg2.pitch_rate = self.pitch_rate #msg2.yaw_rate = self.yaw_rate #msg2.thrust = self.thrust #msg2.kill = 0 #self.bag.write('motors', msg1) #self.bag.write('commands', msg2) #self.pitch = mult*60 #self.thrust = 40000 #self._cf.commander.send_setpoint(0, self.pitch, 0, self.thrust) else: self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self._cf.close_link() self.bag.close() 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 CrazyTracker: def __init__(self): self._log = False; self._cv = False; if self._cv: # Init cameras self._camera_devices = [0, 1] self._w = 640 self._h = 480 self._camera = {} self._pos = {} self._z_orig = -300; self._x_orig = 0; self._y_orig = 0; # Camera properties self._clip = 620 for i in self._camera_devices: self._camera[i] = cv2.VideoCapture(i) self._camera[i].set(cv2.CAP_PROP_FRAME_WIDTH, self._w) self._camera[i].set(cv2.CAP_PROP_FRAME_HEIGHT, self._h) self._pos[i] = ((0,0),0) self._camera[i].set(cv2.CAP_PROP_EXPOSURE, -6) # Overlay to see thresholds self._overlay = np.zeros((self._h,self._w, 3), np.uint8) self._overlay[:,:,:] = (255, 221, 201) # Create controlpanel cv2.namedWindow('Controls') cv2.createTrackbar('H-Value','Controls',251,255,nothing) cv2.createTrackbar('H-Range','Controls',15,128,nothing) cv2.createTrackbar('S-Low','Controls',102,255,nothing) cv2.createTrackbar('S-High','Controls',255,255,nothing) cv2.createTrackbar('V-Low','Controls',95,255,nothing) cv2.createTrackbar('V-High','Controls',245,255,nothing) cv2.createTrackbar('Gauss','Controls',10,50,nothing) cv2.createTrackbar('MinSize','Controls',4,96,nothing) cv2.createTrackbar('Overlay', 'Controls',0,1,nothing) cv2.createTrackbar('Reset', 'Controls',0,1,nothing) cv2.createTrackbar('ON/OFF', 'Controls',0,1,nothing) cv2.createTrackbar('Start', 'Controls',0,1,nothing) cv2.createTrackbar('Land', 'Controls',0,1,nothing) cv2.createTrackbar('Cancel', 'Controls',0,1,nothing) cv2.createTrackbar('Command', 'Controls',0,1,nothing) cv2.createTrackbar('Param', 'Controls',0,255,nothing) cv2.resizeWindow('Controls', 350, 300) self._font = cv2.FONT_HERSHEY_SIMPLEX 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 _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 run(self): # Init Crazyflier cflib.crtp.init_drivers(enable_debug_driver=False) print "Scanning interfaces for Crazyflies..." available = cflib.crtp.scan_interfaces() time.sleep(2) #Some USB bugfix print "Crazyflies found:" for i in available: print i[0] uri = available[-1][0] cf = Crazyflie() cf.open_link(uri) time.sleep(3) #TODO: use the callback instead cf.commander.send_setpoint(0, 0, 0, 0) # Init image dictionaries if self._cv: img = {} img_orig = {} img_blur = {} img_hsv = {} mask_hsv = {} mask_hsv_inv = {} img_masked = {} img_tinted = {} if self._log: self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=100) self._lg_stab.add_variable("stabilizer.pitch", "float") self._lg_stab.add_variable("stabilizer.yaw", "float") self._lg_stab.add_variable("stabilizer.roll", "float") try: cf.log.add_config(self._lg_stab) 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._lg_stab.data_received_cb.add_callback(self._stab_log_data) while True: # Read controlpanel h_value = cv2.getTrackbarPos('H-Value', 'Controls') h_range = cv2.getTrackbarPos('H-Range', 'Controls') s_low = cv2.getTrackbarPos('S-Low', 'Controls') s_high = cv2.getTrackbarPos('S-High', 'Controls') v_low = cv2.getTrackbarPos('V-Low', 'Controls') v_high = cv2.getTrackbarPos('V-High', 'Controls') gauss = cv2.getTrackbarPos('Gauss','Controls') * 2 + 1 min_size = cv2.getTrackbarPos('MinSize','Controls') + 1 overlay = cv2.getTrackbarPos('Overlay','Controls') onoff = cv2.getTrackbarPos('ON/OFF','Controls') z_target = cv2.getTrackbarPos('Height','Controls') show_overlay= cv2.getTrackbarPos('Overlay','Controls') == 1 reset = cv2.getTrackbarPos('Reset','Controls') == 1 start = cv2.getTrackbarPos('Start','Controls') == 1 land = cv2.getTrackbarPos('Land','Controls') == 1 cancel = cv2.getTrackbarPos('Cancel','Controls') == 1 command = cv2.getTrackbarPos('Command','Controls') == 1 cmdParam = cv2.getTrackbarPos('Param','Controls') if start: cv2.setTrackbarPos('Start','Controls',0) if land: cv2.setTrackbarPos('Land','Controls',0) if cancel: cv2.setTrackbarPos('Cancel','Controls',0) if command: cv2.setTrackbarPos('Command','Controls',0) #default xyz sendx = 0 sendy = 0 sendz = 0 # Camera vision code if self._cv: h_min = h_value - h_range h_max = h_value + h_range for i in self._camera_devices: self._camera[i].grab() for i in self._camera_devices: _, img_orig[i] = self._camera[i].retrieve() img_blur[i] = cv2.GaussianBlur(img_orig[i], (gauss,gauss), 0) img_hsv[i] = cv2.cvtColor(img_blur[i], cv2.COLOR_BGR2HSV_FULL) # Take care of region split for hue (red warps around 255) if h_min < 0 or h_max > 255: if h_min < 0: h_upper = h_min + 255 h_lower = h_max elif h_max > 255: h_upper = h_min h_lower = h_max - 255 mask_lower1 = np.array([h_upper, s_low, v_low],np.uint8) mask_upper1 = np.array([255, s_high, v_high],np.uint8) mask_hsv_lower = cv2.inRange(img_hsv[i], mask_lower1, mask_upper1) mask_lower2 = np.array([0, s_low, v_low],np.uint8) mask_upper2 = np.array([h_lower, s_high, v_high],np.uint8) mask_hsv_upper = cv2.inRange(img_hsv[i], mask_lower2, mask_upper2) mask_hsv[i] = cv2.bitwise_or(mask_hsv_lower, mask_hsv_upper) else: mask_lower = np.array([h_min, s_low, v_low],np.uint8) mask_upper = np.array([h_max, s_high, v_high],np.uint8) mask_hsv[i] = cv2.inRange(img_hsv[i], mask_lower, mask_upper) # close and open mask (remove small objects) kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(min_size,min_size)) mask_hsv[i] = cv2.morphologyEx(mask_hsv[i], cv2.MORPH_CLOSE, kernel) mask_hsv[i] = cv2.morphologyEx(mask_hsv[i], cv2.MORPH_OPEN, kernel) img_masked[i] = cv2.bitwise_and(img_blur[i], img_blur[i], mask = mask_hsv[i]) #IMPORTANT: This row will change the mask to 0-1 instead of 0-255 and also there will be some countour lines if you invert the mask _, contours, hierarchy = cv2.findContours(mask_hsv[i], cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) # Find largest contour (we might need to change this to a range) max_area = 0 largest_contour = None for idx, contour in enumerate(contours): area = cv2.contourArea(contour) if area > max_area: max_area = area largest_contour = contour if not largest_contour == None: moment = cv2.moments(largest_contour) self._pos[i] = cv2.minEnclosingCircle(largest_contour) found = True else: found = False #NOT NICE CODE!! DON'T LOOK HERE!!! if show_overlay: mask_hsv_inv[i] = 1 - mask_hsv[i] #Invert now for nice effect img_tinted[i] = cv2.addWeighted(img_orig[i], 0.2, self._overlay, 0.8, 0) cv2.bitwise_xor(img_tinted[i], img_tinted[i], img_tinted[i], mask = mask_hsv[i]) cv2.bitwise_xor(img_orig[i], img_orig[i], img_orig[i], mask = mask_hsv_inv[i]) img[i] = cv2.add(img_tinted[i], img_orig[i]) else: img[i] = img_orig[i] x = int(self._pos[i][0][0]) y = int(self._pos[i][0][1]) r = int(self._pos[i][1]) if found: cv2.circle(img[i], (x, y), r, (255, 0, 255), 2) #fix xyz if found otherwise turn the quad off for security reasons (if cv is disabled the quad will not be turned off) if found: x1 = self._pos[0][0][0] x2 = self._pos[1][0][0] y1 = self._pos[0][0][1] y2 = self._pos[1][0][1] B = 280 D = x2 - x1 f = self._clip z = f*B/D x = {} y = {} x[0] = (x1-self._w/2)*z/f y[0] = (self._h-y1)*z/f x[1] = (x2-self._w/2)*z/f y[1] = (self._h-y2)*z/f if (reset): self._z_orig = z self._x_orig = x[0] self._y_orig = y[0] sendx = int(x[0] - self._x_orig) sendy = int(z - self._z_orig) sendz = int(y[0] - self._y_orig) else: onoff = False; # Flags from C-code #define ON_FLAG 0x1 #define START_FLAG 0x02 #define LAND_FLAG 0x04 #define CANCEL_FLAG 0x08 #define COMMAND_FLAG 0x10 #define PARAM_FLAG 0xFF00 #TODO: maybe change to custom send function (but since this is the exact type of arguments we want we can use it for now i guess) modeFlags = onoff | start << 1 | land << 2 | cancel << 3 | command << 4 | cmdParam << 8 cf.commander.send_setpoint(sendx, sendy, sendz, modeFlags) if self._cv: for i in self._camera_devices: if not show_overlay and found: cv2.putText(img[i], 'XYZ:('+`sendx` + ';' + `sendy`+';'+`sendz`+')' , (10, self._h-15), self._font, 1, (0, 255, 255), 1, cv2.LINE_AA) cv2.imshow("Video" + `i`, img[i]) if cv2.waitKey(1) == 27: cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) cf.close_link() cv2.destroyAllWindows() for i in self._camera_devices: self._camera[i].release() break
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 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 """ # 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("radio://0/2/250K") #self._cf.open_link("radio://0/8/250K") # 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 # The definition of the logconfig can be made before connecting 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") # 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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") Thread(target=self._ramp_motors).start() # 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 from the log API when data arrives""" #print "[%d][%s]: %s" % (timestamp, logconf.name, data) global datas datas = str(data) #print datas 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): global datas global leap_data global run_flag thrust_mult = 1 thrust_step = 2000 thrust = 10000 pitch = 0 del_pitch = 5 roll = 0 del_roll = 5 yawrate = 0 old_leap_data = [0,0,0,0] old_leap_data[0] = 0 old_leap_data[1] = 0 old_leap_data[2] = 0 old_leap_data[3] = 0 pygame.init() screen = pygame.display.set_mode((480*2, 360)) name = "" font = pygame.font.Font(None, 50) while True: #print datas for evt in pygame.event.get(): if evt.type == KEYDOWN: if evt.key == K_BACKSPACE: name = name[:-1] elif evt.key == K_RETURN: name = "" elif evt.key == K_LEFT: roll -= del_roll #name = str(roll) elif evt.key == K_RIGHT: roll += del_roll #name = str(roll) elif evt.key == K_DOWN: pitch -= del_pitch #name = str(pitch) elif evt.key == K_UP: pitch += del_pitch #name = str(pitch) elif evt.key == K_w: thrust += thrust_step #name = str(thrust) elif evt.key == K_s: thrust -= thrust_step #name = str(thrust) elif evt.key == K_q: run_flag = not run_flag elif evt.key == K_ESCAPE: pygame.event.post(pygame.event.Event(QUIT)) elif evt.type == QUIT: self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self._cf.close_link() return if abs(leap_data[0]) > 10.0 and abs(leap_data[0])<60.0: pitch_com = -leap_data[0]/2.0 elif leap_data[0] > 60.0: pitch_com = -30.0 elif leap_data[0] < -60.0: pitch_com = 30.0 else: pitch_com = 0 if abs(leap_data[1]) > 10.0 and abs(leap_data[1])<60.0: roll_com = -leap_data[1]/2.0 elif leap_data[1]>60.0: roll_com = -30.0 elif leap_data[1]<-60.0: roll_com = 30.0 else: roll_com = 0 if abs(pitch_com) < 10.0 and abs(roll_com) < 10.0 and abs(leap_data[2]) > 20: pitch_com = 0.0 roll_com = 0.0 yaw_com = leap_data[2]/5.0 else: yaw_com = 0 if leap_data[3] > 0 and leap_data[3] < 10: thrust_com = 10000 + leap_data[3] * 2000 if leap_data[3] >=10 and leap_data[3] <=1000: thrust_com = 29500 + leap_data[3] * 50 elif leap_data[3] <= 0: thrust_com = 10000 elif leap_data[3] > 1000: thrust_com = 10000 pitch = (0.707*(roll_com + pitch_com)) roll = (0.707*(roll_com - pitch_com)) yawrate = yaw_com if run_flag: thrust = thrust_com else: thrust = 10000 #pitch = pitch_com #roll = roll_com name = "Pitch: " name += str(int(pitch_com)) name += ", Roll: " name += str(int(roll_com)) name += ", Thrust: " name += str(int(thrust_com)) name += ", Yawrate: " name += str(int(yaw_com)) name += ", Live: " name += str(run_flag) screen.fill ((0, 0, 0)) block = font.render(name, True, (255, 255, 255)) rect = block.get_rect() rect.center = screen.get_rect().center screen.blit(block, rect) pygame.display.flip() if thrust < 10001: thrust = 10000 elif thrust > 60000: thrust = 60000 self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1)
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 logs: def __init__(self, cf): #local copy of crazy_Auto self._cf = cf # Roll, Pitch, Yaw self.attitude = [0, 0, 0] # X, Y, Z self.position = [0, 0, 0] # Vx, Vy, Vz self.velocity = [0, 0, 0] self._cf._cf.connected.add_callback(self._init_flight_var) def _init_flight_var(self, link_uri): print("Connected to %s" % link_uri) self.RPY_log = LogConfig(name="Stabilizer", period_in_ms=10) self.RPY_log.add_variable("stabilizer.roll", "float") self.RPY_log.add_variable("stabilizer.pitch", "float") self.RPY_log.add_variable("stabilizer.yaw", "int16_t") self.RPY_log.add_variable('stabilizer.thrust', 'float') self._cf._cf.log.add_config(self.RPY_log) self.RPY_log.data_received_cb.add_callback(self.update_attitude) self.RPY_log.error_cb.add_callback(self.update_error) self.RPY_log.start() # self.quaternion.start() print("Logging Started\n") # optitrack stuff self.l_odom = list() self.l_index = -1 self.sampleInterval = 2 self.s1 = threading.Semaphore(1) # self.streamingClient = NatNetClient("192.168.1.113") # Net2 self.streamingClient = NatNetClient("172.16.5.205") # Net1 # self.streamingClient.rigidBodyListener = self.receiveRigidBodyFrame self.streamingClient.rigidBodyListListener = self.receiveRigidBodyFrame self.streamingClient.run() time.sleep(1) self._cf._cf.connected.add_callback(self._cf._connected) def update_error(self, logconf, msg): print("Error when logging %s: %s" % (logconf.name, msg)) def update_attitude(self, timestamp, data, logconf): # print(data) self.attitude[0] = data["stabilizer.roll"] self.attitude[1] = data["stabilizer.pitch"] self.attitude[2] = data["stabilizer.yaw"] def receiveRigidBodyFrame(self, rigidBodyList, timestamp): # self.rigidBodyList.append((id, pos, rot, trackingValid)) id = rigidBodyList[0][0] pos = rigidBodyList[0][1] rot = rigidBodyList[0][2] trackingValid = rigidBodyList[0][3] # print("stamp: ", timestamp) msg = {'position': [0., 0., 0.], 'stamp': 0, 'velocity': [0., 0., 0.]} msg['stamp'] = timestamp msg['position'][0] = pos[0] msg['position'][1] = pos[1] msg['position'][2] = pos[2] self.s1.acquire() deltatime = 1 if len(self.l_odom) == self.sampleInterval: last_index = (self.l_index + 2) % self.sampleInterval last_msg = self.l_odom[last_index] deltatime = msg['stamp'] - last_msg['stamp'] msg['velocity'][0] = (pos[0] - last_msg['position'][0]) / deltatime msg['velocity'][1] = (pos[1] - last_msg['position'][1]) / deltatime msg['velocity'][2] = (pos[2] - last_msg['position'][2]) / deltatime if abs(msg['velocity'][0]) < 0.0001: msg['velocity'][0] = 0 if abs(msg['velocity'][1]) < 0.0001: msg['velocity'][1] = 0 else: self.l_odom.append(msg) self.l_index = (self.l_index + 1) % self.sampleInterval self.l_odom[self.l_index] = msg self.position[0] = msg['position'][0] self.position[1] = msg['position'][1] self.position[2] = msg['position'][2] self.velocity[0] = msg['velocity'][0] self.velocity[1] = msg['velocity'][1] self.velocity[2] = msg['velocity'][2] self.s1.release()
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 TestFlight: roll = 0 pitch = 0 yawrate = 0 thrust = 0 hold = "False" trimmed_roll = 0 trimmed_pitch = 0 def __init__(self): """ Initialize the quadcopter """ self.f = open('log.log', 'w') self.starttime = time.time() * 1000.0 self.crazyflie = cflib.crazyflie.Crazyflie() print 'Initializing drivers' cflib.crtp.init_drivers() print 'Searching for available devices' available = cflib.crtp.scan_interfaces() radio = False for i in available: # Connect to the first device of the type 'radio' if 'radio' in i[0]: radio = True dev = i[0] print 'Connecting to interface with URI [{0}] and name {1}'.format( i[0], i[1]) self.crazyflie.open_link(dev) break if not radio: print 'No quadcopter detected. Try to connect again.' exit(-1) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): """ Set the loggers """ # Log stabilizer self.logStab = LogConfig("Stabalizer", 200) self.logStab.add_variable("stabilizer.roll", "float") self.logStab.add_variable("stabilizer.pitch", "float") self.logStab.add_variable("stabilizer.yaw", "float") self.logStab.add_variable("stabilizer.thrust", "uint16_t") self.crazyflie.log.add_config(self.logStab) if self.logStab.valid: self.logStab.data_received_cb.add_callback(self.print_stab_data) self.logStab.start() else: print 'Could not setup log configuration for stabilizer after connection!' Thread(target=self.increasing_step).start() Thread(target=self.pulse_command).start() def print_stab_data(self, ident, data, logconfig): #sys.stdout.write('Stabilizer: Roll={1:.2f}, Pitch={2:.2f}\r'.format(data["stabilizer.roll"], data["stabilizer.pitch"])) #sys.stdout.flush() trim_roll = (-1) * data["stabilizer.roll"] + 3.2 trim_pitch = (-1) * data["stabilizer.pitch"] - 0.2 if trim_roll != 0 or trim_pitch != 0: self.trimmed_roll = self.roll + trim_roll self.trimmed_pitch = self.pitch + trim_pitch def increasing_step(self): while 1: command = raw_input( "Set thrust (0-100)% (0 will turn off the motors, e:") if (command == "e"): # Exit the main loop self.hold = "False" print "Exiting main loop in 1 second" time.sleep(0.5) self.crazyflie.close_link( ) # This errors out for some reason. Bad libusb? elif (command == "h"): if self.hold == "True": self.hold = "False" self.thrust = 42500 print "You're NOT in Hover Mode" else: self.hold = "True" self.thrust = 32767 print "You're in Hover Mode" elif (self.is_number(command)): # Good thrust value self.thrust_a = (int(command)) if self.thrust_a <= 100: if int(command) == 0: self.hold = "False" self.thrust = (int(command) * 555 + 10000) print "Setting thrust to %i" % (int(command)) else: print "Remember. Enter a number (0 - 100) or e to exit" def pulse_command(self): while 1: self.crazyflie.param.set_value('flightmode.althold', self.hold) self.crazyflie.commander.send_setpoint(self.trimmed_roll, self.trimmed_pitch, self.yawrate, self.thrust) time.sleep(0.1) def is_number(self, s): try: int(s) return True except ValueError: return False
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 """ # 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) # The definition of the logconfig can be made before connecting 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") # 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.") # 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)) 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 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 """ # 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) self._configured = False self._exit = False # Try to connect to the Crazyflie self._cf.open_link(link_uri) raw_input("Configuring magnometer. Press any key to stop\n") self._configured = True raw_input("Press any key to stop\n") self._exit = 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) # The definition of the logconfig can be made before connecting 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") self._lg_stab.add_variable("baro.aslRaw", "float") ''' self._lg_stab.add_variable("mag.x", "float") self._lg_stab.add_variable("mag.y", "float") self._lg_stab.add_variable("mag.z", "float") self._min_x = self._min_y = self._min_z = 99.9 # 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. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # 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() else: print("Could not add logconfig since some variables are not in TOC") Thread(target=self._work).start() def _work(self): print("Work starting") while not self._configured or not self._exit: time.sleep(0.1) print("Work stopped") 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 from the log API when data arrives""" # print "[%d][%s]: %s" % (timestamp, logconf.name, data) if not self._configured: self._min_x = min(data["mag.x"], self._min_x) self._min_y = min(data["mag.y"], self._min_y) self._min_z = min(data["mag.z"], self._min_z) print("\r({x:=8},{y:=8},{z:=8})) [{x_min:=8},{y_min:=8},{z_min:=8}]"\ .format(\ x=data["mag.x"],\ y=data["mag.y"],\ z=data["mag.z"],\ x_min=self._min_x,\ y_min=self._min_y,\ z_min=self._min_z),end="") else: print("\r({x},{y},{z})"\ .format(\ x=data["mag.x"] - self._min_x,\ y=data["mag.y"] - self._min_y,\ z=data["mag.z"] - self._min_z),end="") 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)