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')
