def yawBeta(ml, sem, kill, angle, rate=20, direction=1, relative=1): try: log = getLogger("Movement") log.info("Yawing " + ("clockwise by " if (direction == 1) else "Counterclockwise by ") + str(angle) + " degrees at " + str(rate) + " deg/s in " + ("relative" if (relative == 1) else "Absolute") + " mode.") ml.mav.command_long_send( ml.target_system, ml.target_component, mavutil.mavlink.MAV_CMD_CONDITION_YAW, 0, # Confirmation angle, # param 1: target angle (deg) rate, # param 2: angular speed (deg/s) direction, # param 3: direction (-1=ccw, 1=cw) relative, # param 4: 0 = absolute, 1 = relative 0, # param 5: Empty 0, # param 6: Empty 0) # param 7: Empty if kill.wait(timeout=((angle / rate) + .5)): # Check if killEvent has been set log.trace("Function yawBeta with angle=" + str(angle) + ", rate=" + str(rate) + ", direction=" + str(direction) + ", relative=" + str(relative) + " was prematurely halted") return # Stop executing function finally: sem.release()
def diveTime(mcParams, sem, kill, time, throttle): ''' :param time: the number of seconds to power the thrusters :param throttle: Throttle value is from -100 to 100, with negative indicating down ''' try: log = getLogger("Movement") log.info("Diving at " + str(throttle) + "% throttle for " + str(time) + " seconds") # set movement parameters mcParams['z'] = (throttle * 5) + 500 # wait if kill.wait(timeout=time): # if killed log.trace("Function diveTime with throttle=" + str(throttle) + ", t=" + str(time) + " was prematurely halted") finally: # reset movement parameters mcParams['z'] = 500 sem.release() log.trace('diveTime ended')
def setFlightMode(ml, sem, mode): # set flight mode try: # mode = "Depth Hold" mode = mode.upper() if mode in ("MANUAL", "CIRCLE", "GUIDED", "ACRO", "ALT_HOLD", "POS_HOLD", "STABILIZE", "AUTO"): pass elif "DEPTH" in mode and "HOLD" in mode: mode = "ALT_HOLD" elif "STABIL" in mode: mode = "STABILIZE" elif "POSITION" in mode: mode = "POS_HOLD" else: print( "Sorry, that mode does not exist. Valid flight modes are: " + "MANUAL, CIRCLE, GUIDED, ACRO, ALT_HOLD, POS_HOLD, STABILIZE, AUTO" ) return log = getLogger("Movement") log.info("Setting Flight Mode to " + str(mode)) ml.set_mode(mode) finally: sem.release()
def move3d(mcParams, sem, kill, throttleX, throttleY, throttleZ, time): '''Throttle functions are integers from -100 to 100''' try: log = getLogger("Movement") log.info("Moving in direction X=" + str(throttleX) + " Y=" + str(throttleY) + " Z=" + str(throttleZ) + " for " + str(time) + " seconds") # Set the movement parameters mcParams['x'] = 10 * throttleX mcParams['y'] = 10 * throttleY mcParams['z'] = 5 * throttleZ + 500 # Wait if kill.wait(timeout=time): # if killed log.trace("Function Move3d with x=" + str(throttleX) + ", y=" + str(throttleX) + ", z=" + str(throttleZ) + ", t=" + str(time) + " was prematurely halted") finally: # Return movement params to normal (but only those that were modified) mcParams['x'] = 0 mcParams['y'] = 0 mcParams['z'] = 500 sem.release() log.trace('move3d ended')
def wait(ml, sem, kill, time): try: log = getLogger("Movement") log.warning("########## DEPRECATION WARNING ##########") log.warning("Wait function is deprecated, use sleep instead.") kill.wait(timeout=time) finally: sem.release()
def __leakDetector(self, killEvent: Event) -> None: '''This function continuously checks for leaks, and upon detecting a leak, runs the desired action''' log = getLogger('Status', doPrint=True) log.trace('Leak Detector started') while not killEvent.wait(timeout=1): if 'STATUSTEXT' in self.messages and 'LEAK' in str( self.messages['STATUSTEXT']['message']).upper(): log.error('Leak Detected: ' + self.messages['STATUSTEXT']['message'] ) # Write the message to the log self.leakResponse() log.trace('StatusMonitor Stopping')
def gripperOpen(mcParams, sem, time): try: log = getLogger('gripper') if time > 2: time = 1.75 log.trace('opening gripper for ' + str(time) + ' seconds') # use a bitwise or to avoid altering other buttons mcParams['b'] = mcParams['b'] | 1 << 10 sleep(time) finally: mcParams['b'] = mcParams['b'] & (~(1 << 10)) sem.release()
def __init__(self, mli): # Keep pointer to main class self.__mli = mli self.__log = getLogger('Mission') # Create pymavlink waypoint loader self.wp = mavwp.MAVWPLoader() # Initialize message counter to 1 self.seq = 1 # idk, but it's important. probably absolute vs relative altitude self.__frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT # First command must be setHome TODO: figure out why self.setHome() self.__log.trace('New Mission object created')
def arm(ml, sem): try: log = getLogger("Status") log.info("Sending arming signal") ml.mav.command_long_send( ml.target_system, ml.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, # Confirmation 1, # param1: 1 = arm 0, # param2: Meaningless 0, # param3: Meaningless 0, # param4: Meaningless 0, # param5: Meaningless 0, # param6: Meaningless 0) # param7: Meaningless finally: sem.release()
def __init__(self): self.disabled = False self.log = getLogger('sonar') self.log.trace('Sonar sensor initialization started') # set address self.address = ("192.168.2.2", 9090) # Configure Socket self.__sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.__sock.settimeout(1.0) # create pingmessage parser self.__parser = pingmessage.PingParser() self.log.trace('Sonar sensor initialization completed') self.log.trace( 'Note that this does not guarantee a working sonar sensor is attached' )
def yawBasic(mcParams, sem, kill, angle, absolute=False): try: log = getLogger("Movement") log.info("Yawing by " + str(angle) + " degrees, absolute: " + str(absolute)) mcParams['r'] = int(angle * (50 / 9)) if kill.wait(timeout=(abs(int(angle * (50 / 9))) / 200)): # Check if killEvent has been set log.trace("Function yaw with angle=" + str(angle) + ", absolute=" + str(absolute) + " was prematurely halted") finally: # reset movement parameters mcParams['r'] = 0 sem.release() log.trace('yawTime ended')
def move(mcParams, sem, kill, direction, time, throttle=50): ''' Modes the drone in 2 dimensions :param direction: The angle (from -180 to 180) at which to move the drone :param time: The number of seconds to thrust for :param throttle: The percentage of total thrust to use ''' try: log = getLogger("Movement") log.info("Moving in direction: " + str(direction) + " at " + str(throttle) + "% throttle" + " for " + str(time) + " seconds") # Calculate x and y values x = cos(pi * direction / 180) y = sin(pi * direction / 180) scaler = (1000 / max(abs(x), abs(y))) * (throttle / 100) x = round(x * scaler) y = round(y * scaler) # Set movement parameters mcParams['x'] = x mcParams['y'] = y # wait if kill.wait(timeout=time): # If killed log.trace("Function Move with direction=" + str(direction) + ", throttle=" + str(throttle) + ", t=" + str(time) + " was prematurely halted") finally: # Reset movement parameters mcParams['x'] = 0 mcParams['y'] = 0 sem.release() log.trace('move ended')
def __updateMessage(self, killEvent: Event) -> None: ''' This function automatically updates a variable to contain the contents of a mavlink message :param killEvent: set killEvent event to end this thread ''' log = getLogger('Refresh') # Log that this was started log.trace('dataRefresher Class Initiating.') filePath = abspath(expanduser("~/logs/mavlinkInterface/")) files = {} for m in self.readMessages: if m in self.recordedMessages: files[m] = open(filePath + '/' + m + '.log', 'a+') while not killEvent.is_set(): # When killEvent is set, stop looping msg = None msg = self.mavlinkConnection.recv_match(type=self.readMessages, blocking=True, timeout=1) # Timeout used so it has the chance to notice the stop flag when no data is present if msg: self.messages[str(msg.get_type())] = { 'message': msg, 'time': datetime.now() } if msg.get_type( ) in self.recordedMessages and self.recordedMessages[ msg.get_type()] == 0: files[msg.get_type()].write( str(datetime.now()) + ', ' + str(msg.to_dict()) + '\n') # when done, ensure that the buffer is written to the files for file in files: file.flush()
def __init__(self, mli): for i in range(0, self.steps): self.__down(mli) sleep(0.04) self.level = 0 self.log = getLogger("Lights")
def __init__(self, execMode: str, sitl=False): ''' Creates a new mavlinkInterface Object :param execMode: The Execution mode to use when not given as a parameter. See docs/configuration/setDefaultexecMode for details.\n ''' execMode = execMode.lower() if execMode not in ['synchronous', 'queue', 'ignore', 'override']: raise ValueError( 'The execMode parameter must be one of the following:\n' + 'synchronous, queue, ignore, override') # Initialize logger self.__log = getLogger('Main', doPrint=True) self.__log.trace( '################################################################################' ) self.__log.trace('###################### New Log ' + str(datetime.now()) + ' ######################') self.__log.trace( '################################################################################' ) # Import config values self.config = ConfigParser() self.configPath = abspath(expanduser('~/.mavlinkInterface.ini')) if exists(self.configPath): self.__log.trace('importing configuration file from path: ' + self.configPath) self.config.read(self.configPath) if (not exists(self.configPath) or 'version' not in self.config or self.config['version']['version'] != self.configVersion): # Populate file with Default config options self.config['version'] = {'version': self.configVersion} self.config['mavlink'] = {'connectionString': 'udp:0.0.0.0:14550'} self.config['geodata'] = { 'COMMENT_1': 'The pressure in pascals at the surface of the body of water.', 'COMMENT_1B': 'Sea Level is around 101325. Varies day by day', 'surfacePressure': '101325', 'COMMENT_2': 'The density of the diving medium. Pure water is 1000', 'fluidDensity': '1000' } self.config['messages'] = { 'refreshrate': '0.04', 'controlRate': '.1' } self.config['hardware'] = {'sonarcount': '1', 'gps': 'True'} # Save file self.config.write((open(self.configPath, 'w'))) # Set class variables self.__log.trace('Setting class variables') self.execMode = execMode self.externalPressureMessage = 'SCALED_PRESSURE2' self.leakResponseAction = 'surface' # Leak response action valid options: # "nothing": warns the user and takes note in the log, but takes no other action # "surface": raises the drone to the surface, overriding any existing commands # <absolute path to .py file, in specified format> (NYI) # Handle SITL Mode self.sitl = sitl if sitl: self.__log.warn('========================================') self.__log.warn('=========== SITL MODE ACTIVE ===========') self.__log.warn('========================================') self.externalPressureMessage = 'SCALED_PRESSURE' # Create variables to contain mavlink message data self.messages = {} self.gpsEnabled = bool(self.config['hardware']['gps']) # Create Semaphore and Queue self.sem = Semaphore(1) self.q = Queue() # Set up Mavlink self.__log.trace('Initializing MavLink Connection') self.mavlinkConnection = mavutil.mavlink_connection( self.config['mavlink']['connectionString']) self.mavlinkConnection.wait_heartbeat() # Start Heartbeat self.mavlinkConnection.mav.request_data_stream_send( # Request start of message stream self.mavlinkConnection.target_system, self.mavlinkConnection.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 0x5, 1) # Building Kill Events self.killEvent = Event( ) # When set, will signal all attached tasks to stop self.currentTaskKillEvent = Event( ) # When set, will kill the current task # Set messages to be read self.readMessages = [ 'SYS_STATUS', 'RAW_IMU', 'SCALED_PRESSURE', 'SCALED_PRESSURE2', 'HEARTBEAT', 'ATTITUDE', 'STATUSTEXT' ] if self.gpsEnabled: self.readMessages.append('GPS_RAW_INT') # Basic GPS self.readMessages.append('GLOBAL_POSITION_INT') # Advanced GPS self.readMessages.append('MISSION_REQUEST') # For missions self.readMessages.append('MISSION_ACK') # For missions self.readMessages.append('MISSION_ITEM') # For missions self.readMessages.append('MISSION_ITEM_REACHED') # For missions self.readMessages.append('MISSION_CURRENT') # For missions self.readMessages.append( 'EKF_STATUS_REPORT') # For GPS and missions # start dataRefreshers self.recordedMessages = {'GPS_RAW_INT': 0, 'SCALED_PRESSURE2': 0} self.refresher = Thread(target=self.__updateMessage, args=(self.killEvent, )) self.refresher.daemon = True # Kill on program end self.refresher.start() # start heartbeat maintainer self.heartbeatThread = Thread(target=self.__heartbeatMaintain, args=(self.killEvent, )) self.heartbeatThread.daemon = True # Kill on program end self.heartbeatThread.start() # start Manual control process self.manualControlParams = { 'x': 0, # X-Axis thrust [Range: -1000-1000; Back=-1000, Forward=1000, 0 = No 'forward' thrust] 'y': 0, # Y-Axis thrust [Range: -1000-1000; Left=-1000, Right=1000, 0 = No 'sideways' thrust] 'z': 500, # Z-Axis thrust [Range: -1000-1000; Left=-1000, Up=1000, 500 = No vertical thrust] 'r': 0, # Rotation [Range: -1000-1000; Left=-1000, Right=1000, 0 = No rotational thrust] 'b': 0 # A bitfield representing controller buttons pressed,(use 1 << btn# to activate button) } self.manualControlThread = Thread(target=self.__manualControlMaintain, args=(self.killEvent, )) self.manualControlThread.daemon = True # Kill on program end self.manualControlThread.start() # start leak detection self.leakDetectorThread = Thread(target=self.__leakDetector, args=(self.killEvent, )) self.leakDetectorThread.daemon = True # Kill on program end self.leakDetectorThread.start() # Start Queue maintenance process self.queueThread = Thread(target=self.__queueManager, args=(self.killEvent, )) self.queueThread.daemon = True # Kill on program end self.queueThread.start() # Start message recording process self.dataRecorderThread = Thread(target=self.__dataRecorder, args=(self.killEvent, )) self.dataRecorderThread.daemon = True # Kill on program end self.dataRecorderThread.start() # Initiate light class self.lights = commands.active.lights(self) # Initiate sonar class if int(self.config['hardware']['sonarcount']) > 0: self.sonar = commands.passive.sonar() if self.gpsEnabled: self.gps = commands.passive.gps(self) # Validating heartbeat self.__log.info('Waiting for heartbeat') while 'HEARTBEAT' not in self.messages: sleep(.1) self.__log.info('Successfully connected to target.') self.__log.trace('__init__ end') atexit.register(self.waitQueue)
def surface(mli, kill): # TODO log = getLogger("Movement") log.info("Rising to Surface") dive(mli, kill, 0, throttle=100, absolute=True)
def yaw(mli, kill, angle, absolute=False): # TODO add rotational momentum to calculation ''' Rotates the drone by *angle* degrees Note: This will never yaw by more than 180 degrees :param angle: The change in heading (in degrees), with negative being counterclockwise ''' try: log = getLogger("Movement") log.info("Yawing by " + str(angle) + " absolute=" + str(absolute)) currentHeading = mli.getHeading() if absolute: # In absolute mode, just face toward the input angle targetHeading = angle % 360 else: # relative # In relative mode, yaw by the input angle targetHeading = (angle + currentHeading) % 360 log.trace('yaw: current: ' + str(currentHeading)) log.trace('yaw: target: ' + str(targetHeading)) if (targetHeading - currentHeading) % 360 <= 180: # Clockwise mli.manualControlParams['r'] = 500 while not kill.wait(timeout=.25): # Until within 30 degrees of target, yaw at 50% if (targetHeading - mli.getHeading()) % 360 <= 30: break mli.manualControlParams['r'] = 250 while not kill.wait(timeout=.25): # Until within 10 degrees of target, yaw at 25% if ((targetHeading - mli.getHeading()) % 360 <= 5 or (targetHeading - mli.getHeading()) % 360 > 330): break mli.manualControlParams['r'] = -250 while not kill.wait(timeout=.1): # Cancel momentum if mli.messages['ATTITUDE']['message'].yawspeed < 0.01: break elif (targetHeading - currentHeading) % 360 > 180: # Counterclockwise mli.manualControlParams['r'] = -500 while not kill.wait(timeout=.25): # Until within 30 degrees of target, yaw at 50% if (targetHeading - mli.getHeading()) % 360 > 330: break mli.manualControlParams['r'] = -250 while not kill.wait(timeout=.25): # Until within 10 degrees of target, yaw at 25% if ((targetHeading - mli.getHeading()) % 360 > 355 or (targetHeading - mli.getHeading()) % 360 < 30): break mli.manualControlParams['r'] = 250 while not kill.wait(timeout=.1): # Cancel momentum if mli.messages['ATTITUDE']['message'].yawspeed > -0.01: break finally: # reset yaw control to zero mli.manualControlParams['r'] = 0 mli.sem.release() log.trace('yaw command ended')
def __init__(self, mli): self.mli = mli self.log = getLogger('gps') pass
def dive(mli, kill, depth, throttle=50, absolute=False): ''' :param depth: The change in depth, negative being down :param throttle: Percent of thruster power to use ''' try: if throttle > 75: acceptThreshold = .33 safetyThreshold = .5 elif throttle > 50: acceptThreshold = .15 safetyThreshold = .33 elif throttle > 25: acceptThreshold = .05 safetyThreshold = .15 else: acceptThreshold = 0 safetyThreshold = .05 log = getLogger("Movement") log.info("Diving to depth=" + str(depth) + " at throttle=" + str(throttle) + "% power, absolute=" + str(absolute)) # set depth acceptance and safety thresholds if throttle > 75: acceptThreshold = .33 safetyThreshold = .5 elif throttle > 50: acceptThreshold = .15 safetyThreshold = .33 elif throttle > 25: acceptThreshold = .05 safetyThreshold = .15 else: acceptThreshold = 0 safetyThreshold = .08 # set target depth currentDepth = mli.getDepth() if absolute: # In absolute mode, just go to a depth targetDepth = depth else: # relative # In relative mode, go up/down by a depth targetDepth = depth + currentDepth if targetDepth > 0: log.error("Cannot Rise above the Surface, aborting") raise ValueError("Cannot Rise above the Surface, aborting command") # set movement parameters if currentDepth > targetDepth + acceptThreshold: # Need to descend descend = True mli.manualControlParams['z'] = 500 - (throttle * 5) elif currentDepth < targetDepth - acceptThreshold: # Need to ascend descend = False mli.manualControlParams['z'] = 500 + (throttle * 5) else: log.trace('Already at desired depth') return i = 0 oldDepth = currentDepth while not kill.wait(timeout=0.25): currentDepth = mli.getDepth() # if drone has descended or ascended far enough if descend and currentDepth < targetDepth + acceptThreshold: break if not descend and currentDepth > targetDepth - acceptThreshold: break if i == 12: # every 3 seconds # if the drone has been thrusting for 3 seconds, but has not moved if abs(oldDepth - currentDepth) <= safetyThreshold: log.trace( "Function dive with depth=" + str(depth) + ", throttle=" + str(throttle) + ", absolute=" + str(absolute) + " was prematurely halted due to a lack of movement") break i = 0 oldDepth = currentDepth i += 1 if kill.is_set(): log.trace("Function dive with depth=" + str(depth) + ", throttle=" + str(throttle) + ", absolute=" + str(absolute) + " was prematurely halted") finally: # reset movement parameters mli.manualControlParams['z'] = 500 mli.sem.release() log.trace('Dive function ended')