class Drone: def __init__(self, anchors, database, isMaster): # init the Drone as inactive self.active = False # init drone as Master or Worker self.isMaster = isMaster # init the database self.database = database # init the tag, that tracks the Drone (pozyx) try: self.tag = Tag(anchors) except PozyxConnectionError: print("No Pozyx Tag found - will mock up position for tests.") # if no tag found, set it to None self.tag = None if self.tag is not None: # setup for the Tag self.tag.setup() # init anchors self.anchors = anchors try: from models.yaw_detection import YawDetection # import yawDetection class self.yaw_detector = YawDetection() except ImportError: print("couldn't start camera and opencv") # if opencv fails, set detector None self.yaw_detector = None # class, that communicates with the flight controller self.control = None # current position of the drone (x, y, z) self.position = None # current orientation of the drone (yaw, roll, pitch) self.orientation = None # create a Tag in the database and saves the id of the drone tag objects entity self.tag_id = database.tag.put(TagModel()) # create the Tag entity - easy read and write self.tag_object = database.tag.get(self.tag_id) # init LED-Stick self.led_sticks = LedStick(self.tag_id, database) def startUpdateLoop(self): # set the Drone as active self.active = True while self.active: self.updatePosition() sleep(0.2) if self.position is not None: # saves the position in the database self.savePositionToDatabase() self.tag_object.printPosition() if self.isMaster: self.updateLEDsFromPosition(self.database) self.led_sticks.setColorFromDatabase(self.database) if self.orientation is not None: # saves the orientation in the database self.saveOrientationToDatabase() self.tag_object.printOrientation() def updateLEDsFromPosition(self, database): # get saved postions from database position = database.tag.get(self.tag_id).getPosition() # get the max values from the anchor coordinates max_x = max(self.anchors[0][2], self.anchors[1][2], self.anchors[2][2], self.anchors[3][2]) max_y = max(self.anchors[0][3], self.anchors[1][3], self.anchors[2][3], self.anchors[3][3]) max_z = max(self.anchors[0][4], self.anchors[1][4], self.anchors[2][4], self.anchors[3][4]) # calculate the color depends on the position [coordinate * factor = 255] r = position["x"] * (255/max_x) g = position["y"] * (255/max_y) b = position["z"] * (255/max_z) # set the color values self.led_sticks.updateColorFromPosition(r, g, b) # save color to database self.led_sticks.saveColorToDatabase(database) def updatePosition(self): if self.tag is not None: # update position & orientation from the Tag self.position = self.tag.getPosition() # ToDo: merge OpenCV and Pozyx orientation self.orientation = self.tag.getOrientation() else: # load mocked classes for testing self.position = Tag.mockedPosition() self.orientation = Tag.mockedOrientation() if self.yaw_detector is not None and self.yaw_detector.initVideocapture(): # Merge detected Angle with pozyx Angle print("yaw detector angle: ", self.yaw_detector.getAngle()) def savePositionToDatabase(self): self.tag_object = self.database.tag.get(self.tag_id) self.tag_object.setPosition(self.position.x, self.position.y, self.position.z) # update position in database object self.database.tag.put(self.tag_object) def saveOrientationToDatabase(self): self.tag_object.setOrientation(self.orientation.heading, self.orientation.roll, self.orientation.pitch) # update orientation in database object self.database.tag.put(self.tag_object)