# -*- coding: utf-8 -*- # # || ____ _ __ # +------+ / __ )(_) /_______________ _____ ___ # | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # # Copyright (C) 2017 Bitcraze AB # # Crazyflie Nano Quadcopter Client # # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU General Public License # as published by the Free Software Foundation; either version 2 # of the License, or (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. import logging import math import time import sys import threading import _thread import numpy as np from vispy import scene from vispy.scene import visuals from vispy.scene.cameras import TurntableCamera import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander from cflib.positioning.position_hl_commander import PositionHlCommander from cflib.utils import uri_helper from cflib.utils import multiranger from cflib.utils.multiranger import Multiranger from numpy.core import multiarray from PyQt5 import QtCore, QtWidgets URI = uri_helper.uri_from_env(default='radio://0/83/2M/E7E7E7E7E7') DEFAULT_HEIGHT = 0.5 #BOX_LIMIT = 0.40 is_deck_attached = False logging.basicConfig(level=logging.ERROR) position_estimate = [0, 0] keep_flying = True #----------------------------------------------Visuall Mapping------------------------------ # Enable plotting of Crazyflie PLOT_CF = False # Enable plotting of down sensor PLOT_SENSOR_DOWN = False # Set the sensor threshold (in mm) SENSOR_TH = 2000 # Set the speed factor for moving and rotating SPEED_FACTOR = 0.3 class MainWindow(QtWidgets.QMainWindow): def __init__(self, URI): QtWidgets.QMainWindow.__init__(self) self.resize(700, 500) self.setWindowTitle('Multi-ranger point cloud') self.canvas = Canvas(self.updateHover) self.canvas.create_native() self.canvas.native.setParent(self) self.setCentralWidget(self.canvas.native) cflib.crtp.init_drivers() self.cf = Crazyflie(ro_cache=None, rw_cache='cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} self.hoverTimer = QtCore.QTimer() self.hoverTimer.timeout.connect(self.sendHoverCommand) self.hoverTimer.setInterval(100) self.hoverTimer.start() def sendHoverCommand(self): self.cf.commander.send_hover_setpoint( self.hover['x'], self.hover['y'], self.hover['yaw'], self.hover['height']) def updateHover(self, k, v): if (k != 'height'): self.hover[k] = v * SPEED_FACTOR else: self.hover[k] += v def disconnected(self, URI): print('Disconnected') def connected(self, URI): print('We are now connected to {}'.format(URI)) # The definition of the logconfig can be made before connecting lpos = LogConfig(name='Position', period_in_ms=100) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') try: self.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(self.pos_data) lpos.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') lmeas = LogConfig(name='Meas', period_in_ms=100) lmeas.add_variable('range.front') lmeas.add_variable('range.back') lmeas.add_variable('range.up') lmeas.add_variable('range.left') lmeas.add_variable('range.right') lmeas.add_variable('range.zrange') lmeas.add_variable('stabilizer.roll') lmeas.add_variable('stabilizer.pitch') lmeas.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lmeas) lmeas.data_received_cb.add_callback(self.meas_data) lmeas.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') def pos_data(self, timestamp, data, logconf): position = [ data['stateEstimate.x'], data['stateEstimate.y'], data['stateEstimate.z'] ] self.canvas.set_position(position) def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] } self.canvas.set_measurement(measurement) def closeEvent(self, event): if (self.cf is not None): self.cf.close_link() class Canvas(scene.SceneCanvas): def __init__(self, keyupdateCB): scene.SceneCanvas.__init__(self, keys=None) self.size = 800, 600 self.unfreeze() self.view = self.central_widget.add_view() self.view.bgcolor = '#ffffff' self.view.camera = TurntableCamera( fov=10.0, distance=30.0, up='+z', center=(0.0, 0.0, 0.0)) self.last_pos = [0, 0, 0] self.pos_markers = visuals.Markers() self.meas_markers = visuals.Markers() self.pos_data = np.array([0, 0, 0], ndmin=2) self.meas_data = np.array([0, 0, 0], ndmin=2) self.lines = [] self.view.add(self.pos_markers) self.view.add(self.meas_markers) for i in range(6): line = visuals.Line() self.lines.append(line) self.view.add(line) self.keyCB = keyupdateCB self.freeze() scene.visuals.XYZAxis(parent=self.view.scene) def on_key_press(self, event): if (not event.native.isAutoRepeat()): if (event.native.key() == QtCore.Qt.Key_Left): self.keyCB('y', 1) if (event.native.key() == QtCore.Qt.Key_Right): self.keyCB('y', -1) if (event.native.key() == QtCore.Qt.Key_Up): self.keyCB('x', 1) if (event.native.key() == QtCore.Qt.Key_Down): self.keyCB('x', -1) if (event.native.key() == QtCore.Qt.Key_A): self.keyCB('yaw', -70) if (event.native.key() == QtCore.Qt.Key_D): self.keyCB('yaw', 70) if (event.native.key() == QtCore.Qt.Key_Z): self.keyCB('yaw', -200) if (event.native.key() == QtCore.Qt.Key_X): self.keyCB('yaw', 200) if (event.native.key() == QtCore.Qt.Key_W): self.keyCB('height', 0.1) if (event.native.key() == QtCore.Qt.Key_S): self.keyCB('height', -0.1) def on_key_release(self, event): if (not event.native.isAutoRepeat()): if (event.native.key() == QtCore.Qt.Key_Left): self.keyCB('y', 0) if (event.native.key() == QtCore.Qt.Key_Right): self.keyCB('y', 0) if (event.native.key() == QtCore.Qt.Key_Up): self.keyCB('x', 0) if (event.native.key() == QtCore.Qt.Key_Down): self.keyCB('x', 0) if (event.native.key() == QtCore.Qt.Key_A): self.keyCB('yaw', 0) if (event.native.key() == QtCore.Qt.Key_D): self.keyCB('yaw', 0) if (event.native.key() == QtCore.Qt.Key_W): self.keyCB('height', 0) if (event.native.key() == QtCore.Qt.Key_S): self.keyCB('height', 0) if (event.native.key() == QtCore.Qt.Key_Z): self.keyCB('yaw', 0) if (event.native.key() == QtCore.Qt.Key_X): self.keyCB('yaw', 0) def set_position(self, pos): self.last_pos = pos if (PLOT_CF): self.pos_data = np.append(self.pos_data, [pos], axis=0) self.pos_markers.set_data(self.pos_data, face_color='red', size=5) def rot(self, roll, pitch, yaw, origin, point): cosr = math.cos(math.radians(roll)) cosp = math.cos(math.radians(pitch)) cosy = math.cos(math.radians(yaw)) sinr = math.sin(math.radians(roll)) sinp = math.sin(math.radians(pitch)) siny = math.sin(math.radians(yaw)) roty = np.array([[cosy, -siny, 0], [siny, cosy, 0], [0, 0, 1]]) rotp = np.array([[cosp, 0, sinp], [0, 1, 0], [-sinp, 0, cosp]]) rotr = np.array([[1, 0, 0], [0, cosr, -sinr], [0, sinr, cosr]]) rotFirst = np.dot(rotr, rotp) rot = np.array(np.dot(rotFirst, roty)) tmp = np.subtract(point, origin) tmp2 = np.dot(rot, tmp) return np.add(tmp2, origin) def rotate_and_create_points(self, m): data = [] o = self.last_pos roll = m['roll'] pitch = -m['pitch'] yaw = m['yaw'] if (m['up'] < SENSOR_TH): up = [o[0], o[1], o[2] + m['up'] / 1000.0] data.append(self.rot(roll, pitch, yaw, o, up)) if (m['down'] < SENSOR_TH and PLOT_SENSOR_DOWN): down = [o[0], o[1], o[2] - m['down'] / 1000.0] data.append(self.rot(roll, pitch, yaw, o, down)) if (m['left'] < SENSOR_TH): left = [o[0], o[1] + m['left'] / 1000.0, o[2]] data.append(self.rot(roll, pitch, yaw, o, left)) if (m['right'] < SENSOR_TH): right = [o[0], o[1] - m['right'] / 1000.0, o[2]] data.append(self.rot(roll, pitch, yaw, o, right)) if (m['front'] < SENSOR_TH): front = [o[0] + m['front'] / 1000.0, o[1], o[2]] data.append(self.rot(roll, pitch, yaw, o, front)) if (m['back'] < SENSOR_TH): back = [o[0] - m['back'] / 1000.0, o[1], o[2]] data.append(self.rot(roll, pitch, yaw, o, back)) return data def set_measurement(self, measurements): data = self.rotate_and_create_points(measurements) o = self.last_pos for i in range(6): if (i < len(data)): o = self.last_pos self.lines[i].set_data(np.array([o, data[i]])) else: self.lines[i].set_data(np.array([o, o])) if (len(data) > 0): self.meas_data = np.append(self.meas_data, data, axis=0) self.meas_markers.set_data(self.meas_data, face_color='blue', size=5) #--------------------------------------------------------------------------------------------------------------- def moveTest(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: print('Wir fliegen vor ...') mc.forward(1) time.sleep(2) print("Reseting ...") #Kalmanfilter mc._reset_position_estimator() print('Wir fliegen zurueck ...') mc.back(1) time.sleep(1) def sensorsTest(scf): with Multiranger(scf) as multiranger: while True: if (is_close(multiranger.up)): print('Oben!') if not (is_close(multiranger.front)): time.sleep(0.1) else: print('Vorne') time.sleep(0.1) def move(scf): global keep_flying direction = True with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: with Multiranger(scf) as multiranger: while keep_flying: #Manuelles beenden des Quadrocokters, indem man mit der Hand über die Drohne schweift if (is_close(multiranger.up)): print('Stop Oben!') keep_flying = False mc.stop() else: #Damit die Drohne nicht gegen die Linke Wand fliegt if (is_close(multiranger.left)): mc.right(0.1) time.sleep(0.1) else: if not (is_close(multiranger.front)): mc.start_linear_motion(0.1, 0, 0) time.sleep(0.1) else: #einmal nach rechts drehen, danach links if (direction): mc.turn_right(90) if not (is_close(multiranger.front)): mc.forward(0.2) time.sleep(1) mc.turn_right(90) direction = False else: keep_flying = False else: mc.turn_left(90) if not (is_close(multiranger.front)): mc.forward(0.2) time.sleep(1) mc.turn_left(90) direction = True else: keep_flying = False mc.stop() #Parameter .front .back .left. right .up def is_close(range): MIN_DISTANCE = 0.30 # m if range is None: return False else: return range < MIN_DISTANCE def check_direction(direction): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: if (is_close(direction)): mc.stop() def fly_to_position(scf, x, y): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: with Multiranger(scf) as multiranger: #Zur X Position fliegen difference_x = x - position_estimate[0] if(difference_x > 0): check_direction(multiranger.front) print('Fliegt vor') mc.forward(difference_x/1000) elif (difference_x < 0): check_direction(multiranger.back) print('Fliegt zurück') mc.back((difference_x*(-1))/1000) time.sleep(1) #Zur Y Position fliegen difference_y = y - position_estimate[1] if(difference_y > 0): check_direction(multiranger.right) print('Fliegt rechts') mc.right(difference_y*(-1)/1000) elif (difference_y < 0): check_direction(multiranger.left) print('Fliegt links') mc.left((difference_y)/1000) def param_deck_flow(name, value_str): value = int(value_str) print(value) global is_deck_attached if value: is_deck_attached = True print('Deck is attached!') else: is_deck_attached = False print('Deck is NOT attached!') def log_pos_callback(timestamp, data, logconf): print(data) global position_estimate position_estimate[0] = data['stateEstimateZ.x'] position_estimate[1] = data['stateEstimateZ.y'] def thread_visual(): appQt = QtWidgets.QApplication(sys.argv) win = MainWindow(URI) win.show() appQt.exec_() def thread_move(): move(scf) if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow) time.sleep(10) print('Wir loggen ...') logconf = LogConfig(name='Position', period_in_ms=10) logconf.add_variable('stateEstimateZ.x', 'int16_t') logconf.add_variable('stateEstimateZ.y', 'int16_t') scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) if is_deck_attached: print('Logconf.start() ...') logconf.start() print('move start ...') #thread_visual() #thread_move() x = threading.Thread(target=thread_visual) y = threading.Thread(target=thread_move) x.start() y.start() time.sleep(10) x.join() y.join() #Testflug #moveTest(scf) #sensorsTest(scf) #Orienterungsflug #move(scf) logconf.stop()