diff --git a/.pylintrc b/.pylintrc index b72a43c..d11bb54 100644 --- a/.pylintrc +++ b/.pylintrc @@ -11,7 +11,7 @@ ignore=CVS # Add files or directories matching the regex patterns to the blacklist. The # regex matches against base names, not paths. -ignore-patterns= +# ignore-patterns= # Python code to execute, usually for sys.path manipulation such as # pygtk.require(). @@ -43,7 +43,7 @@ unsafe-load-any-extension=no # Only show warnings with the listed confidence levels. Leave empty to show # all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED -confidence= +# confidence= # Disable the message, report, category or checker with the given id(s). You # can either give multiple identifiers separated by comma (,) or put this @@ -186,13 +186,13 @@ max-spelling-suggestions=4 # Spelling dictionary name. Available dictionaries: none. To make it working # install python-enchant package. -spelling-dict= +# spelling-dict= # List of comma separated words that should not be checked. -spelling-ignore-words= +# spelling-ignore-words= # A path to a file that contains private dictionary; one word per line. -spelling-private-dict-file= +# spelling-private-dict-file= # Tells whether to store unknown words to indicated private dictionary in # --spelling-private-dict-file option instead of raising a message. @@ -216,7 +216,7 @@ contextmanager-decorators=contextlib.contextmanager # List of members which are set dynamically and missed by pylint inference # system, and so shouldn't trigger E1101 when accessed. Python regular # expressions are accepted. -generated-members= +# generated-members= # Tells whether missing members accessed in mixin class should be ignored. A # mixin class is detected if its name ends with "mixin" (case insensitive). @@ -239,7 +239,7 @@ ignored-classes=optparse.Values,thread._local,_thread._local,str # (useful for modules/projects where namespaces are manipulated during runtime # and thus existing member attributes cannot be deduced by static analysis. It # supports qualified module names, as well as Unix pattern matching. -ignored-modules= +ignored-modules=numpy # Show a hint with possible names when a member name was not found. The aspect # of finding the hint is based on edit distance. @@ -258,7 +258,7 @@ missing-member-max-choices=1 # List of additional names supposed to be defined in builtins. Remember that # you should avoid to define new builtins when possible. -additional-builtins= +# additional-builtins= # Tells whether unused global variables should be treated as a violation. allow-global-unused-variables=yes @@ -343,14 +343,14 @@ argument-naming-style=snake_case # Regular expression matching correct argument names. Overrides argument- # naming-style -#argument-rgx= +# argument-rgx= # Naming style matching correct attribute names attr-naming-style=snake_case # Regular expression matching correct attribute names. Overrides attr-naming- # style -#attr-rgx= +# attr-rgx= # Bad variable names which should always be refused, separated by a comma bad-names=foo, @@ -371,14 +371,14 @@ class-attribute-naming-style=any class-naming-style=PascalCase # Regular expression matching correct class names. Overrides class-naming-style -#class-rgx= +# class-rgx= # Naming style matching correct constant names const-naming-style=UPPER_CASE # Regular expression matching correct constant names. Overrides const-naming- # style -#const-rgx= +# const-rgx= # Minimum line length for functions/classes that require docstrings, shorter # ones are exempt. @@ -389,7 +389,7 @@ function-naming-style=snake_case # Regular expression matching correct function names. Overrides function- # naming-style -#function-rgx= +# function-rgx= # Good variable names which should always be accepted, separated by a comma good-names=i, @@ -407,25 +407,25 @@ inlinevar-naming-style=any # Regular expression matching correct inline iteration names. Overrides # inlinevar-naming-style -#inlinevar-rgx= +# inlinevar-rgx= # Naming style matching correct method names method-naming-style=snake_case # Regular expression matching correct method names. Overrides method-naming- # style -#method-rgx= +# method-rgx= # Naming style matching correct module names module-naming-style=snake_case # Regular expression matching correct module names. Overrides module-naming- # style -#module-rgx= +# module-rgx= # Colon-delimited sets of names that determine each other's naming style when # the name regexes allow several styles. -name-group= +# name-group= # Regular expression which should only match function or class names that do # not require a docstring. @@ -440,7 +440,7 @@ variable-naming-style=snake_case # Regular expression matching correct variable names. Overrides variable- # naming-style -#variable-rgx= +# variable-rgx= [IMPORTS] @@ -462,15 +462,15 @@ ext-import-graph= # Create a graph of every (i.e. internal and external) dependencies in the # given file (report RP0402 must not be disabled) -import-graph= +# import-graph= # Create a graph of internal dependencies in the given file (report RP0402 must # not be disabled) -int-import-graph= +# int-import-graph= # Force import order to recognize a module as part of the standard # compatibility libraries. -known-standard-library= +# known-standard-library= # Force import order to recognize a module as part of a third party library. known-third-party=enchant diff --git a/.travis.yml b/.travis.yml index 1391c59..9bfcbd4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,4 +10,4 @@ install: - pip3 install tox-travis script: - - tox -e lint,vtol -c tox.ini \ No newline at end of file + - tox -e lint,hex -c tox.ini \ No newline at end of file diff --git a/README.md b/README.md index 5e9ff22..76e3c5e 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -[![Build Status](https://travis-ci.com/NGCP/VTOL.svg?token=pzpV4AQm2iKJkxHKKsEz&branch=master)](https://travis-ci.com/NGCP/VTOL) +[![Build Status](https://travis-ci.com/NGCP/HEX.svg?token=pzpV4AQm2iKJkxHKKsEz&branch=master)](https://travis-ci.com/NGCP/HEX) diff --git a/archives/detailed_search_autonomy.py b/archives/detailed_search_autonomy.py index 1f95923..ef2f71b 100644 --- a/archives/detailed_search_autonomy.py +++ b/archives/detailed_search_autonomy.py @@ -80,31 +80,31 @@ def orbit_poi(vehicle, poi, configs): waypoints.append(LocationGlobalRelative(lat, lon, alt)) # Go to center of POI - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, waypoint_tolerance, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) - # Transition to quadcopter if applicable - if (configs["vehicle_type"] == "VTOL"): + # Transition to hexcopter if applicable + if (configs["vehicle_type"] == "HEX"): cmds.add( - Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_HEX_TRANSITION, 0, 0, - mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) + mavutil.mavlink.MAV_HEX_STATE_MC, 0, 0, 0, 0, 0, 0)) # Circular waypoints - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, waypoint_tolerance, 0, 0, 0, point.lat, point.lon, point.alt)) - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, @@ -112,11 +112,11 @@ def orbit_poi(vehicle, poi, configs): 0, 0, 0, 0, point.lat, point.lon, point.alt)) # Transition to forward flight if applicable - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): cmds.add( - Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_HEX_TRANSITION, 0, 0, - mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) + mavutil.mavlink.MAV_HEX_STATE_MC, 0, 0, 0, 0, 0, 0)) # Add dummy endpoint cmds.add( @@ -139,15 +139,15 @@ def detailed_search_adds_mission(configs, vehicle): cmds.clear() # Due to a bug presumed to be the fault of DroneKit, the first command is ignored. Thus we have two takeoff commands - if (configs["vehicle_type"] == "VTOL"): - # Separate MAVlink message for a VTOL takeoff. This takes off to altitude and transitions + if (configs["vehicle_type"] == "HEX"): + # Separate MAVlink message for a HEX takeoff. This takes off to altitude and transitions cmds.add( - Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0, + Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_HEX_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) cmds.add( - Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0, + Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_HEX_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) @@ -233,9 +233,9 @@ def detailed_search_autonomy(configs, autonomyToCV, GCS_TIMESTAMP, CONNECTION_TI # Holds the copter in place if receives pause if autonomy.PAUSE_MISSION: - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): vehicle.mode = VehicleMode("QHOVER") - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): vehicle.mode = VehicleMode("ALT_HOLD") change_status("paused") diff --git a/archives/executable_picker.py b/archives/executable_picker.py index 1d26995..88fcc5f 100644 --- a/archives/executable_picker.py +++ b/archives/executable_picker.py @@ -1,13 +1,13 @@ ''' The GCS allocates roles to vehicles. The purpose of this program is to connect to the GCS and then read a start message from the GCS, which contains a certain job_type. It then runs the -corresponding VTOL program (quick scan, detailed search, guide) based on the start message. +corresponding HEX program (quick scan, detailed search, guide) based on the start message. ''' import sys import json import time from util import parse_configs, new_output_file -from autonomy import VTOL +from autonomy import HEX import autonomy XBEE = None diff --git a/archives/quick_scan_autonomy.py b/archives/quick_scan_autonomy.py index 14983a2..12b0b5e 100644 --- a/archives/quick_scan_autonomy.py +++ b/archives/quick_scan_autonomy.py @@ -160,12 +160,12 @@ def quick_scan_adds_mission(configs, vehicle, lla_waypoint_list): print(" Define/add new commands.") # Due to a bug presumed to be the fault of DroneKit, the first command is ignored. Thus we have two takeoff commands - if (configs["vehicle_type"] == "VTOL"): - # Separate MAVlink message for a VTOL takeoff. This takes off to altitude and transitions - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) + if (configs["vehicle_type"] == "HEX"): + # Separate MAVlink message for a HEX takeoff. This takes off to altitude and transitions + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_HEX_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_HEX_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altitude)) @@ -174,7 +174,7 @@ def quick_scan_adds_mission(configs, vehicle, lla_waypoint_list): 0, configs["air_speed"], -1, 0, 0, 0, 0)) # Add waypoints to the auto mission - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): for point in lla_waypoint_list: # Planes need a waypoint tolerance cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, @@ -182,7 +182,7 @@ def quick_scan_adds_mission(configs, vehicle, lla_waypoint_list): # Adds dummy end point - this endpoint is the same as the last waypoint and lets us know we have reached destination. cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, configs["waypoint_tolerance"], 0, 0, lla_waypoint_list[-1].lat, lla_waypoint_list[-1].lon, lla_waypoint_list[-1].alt)) - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): for point in lla_waypoint_list: cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point.lat, point.lon, point.alt)) @@ -259,9 +259,9 @@ def quick_scan_autonomy(configs, autonomyToCV, GCS_TIMESTAMP, CONNECTION_TIMESTA time.sleep(1) # Holds the copter in place if receives pause if autonomy.PAUSE_MISSION: - if (configs["vehicle_type"] == "VTOL"): + if (configs["vehicle_type"] == "HEX"): vehicle.mode = VehicleMode("QHOVER") - elif (configs["vehicle_type"] == "Quadcopter"): + elif (configs["vehicle_type"] == "Hexcopter"): vehicle.mode = VehicleMode("ALT_HOLD") # Lands the vehicle if receives stop mission elif autonomy.STOP_MISSION: diff --git a/comm_sim_example.json b/comm_sim_example.json index 502318c..2d5a587 100644 --- a/comm_sim_example.json +++ b/comm_sim_example.json @@ -1,9 +1,20 @@ [ -{ + { "time": 5.0, "message" : { - "type": "RTL", + "type": "TAKEOFF", "id": 1 } - } + }, + { + "time": 15.0, + "message" : { + "type": "GOTO", + "body": { + "lat": 100, + "lon": 100 + }, + "id": 1 + } + } ] diff --git a/dev_requirements.txt b/dev_requirements.txt index 3dc4385..71e0039 100644 --- a/dev_requirements.txt +++ b/dev_requirements.txt @@ -2,3 +2,4 @@ pylint python-dotenv pytest shapely +black \ No newline at end of file diff --git a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_1.txt b/examples/RCOverride/SITLExampleWaypoints/waypoint_test_1.txt deleted file mode 100644 index b40a7d4..0000000 --- a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_1.txt +++ /dev/null @@ -1,3 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 650.000000 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.356833 149.162703 41.029999 1 diff --git a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_2.txt b/examples/RCOverride/SITLExampleWaypoints/waypoint_test_2.txt deleted file mode 100644 index e9e44a6..0000000 --- a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_2.txt +++ /dev/null @@ -1,3 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 650.000000 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.359124 149.168475 41.029999 1 diff --git a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_3.txt b/examples/RCOverride/SITLExampleWaypoints/waypoint_test_3.txt deleted file mode 100644 index 3929477..0000000 --- a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_3.txt +++ /dev/null @@ -1,3 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 650.000000 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.371669 149.169827 41.029999 1 diff --git a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_4.txt b/examples/RCOverride/SITLExampleWaypoints/waypoint_test_4.txt deleted file mode 100644 index 9a82b5b..0000000 --- a/examples/RCOverride/SITLExampleWaypoints/waypoint_test_4.txt +++ /dev/null @@ -1,3 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 650.000000 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.371415 149.162192 41.029999 1 diff --git a/examples/RCOverride/pid.py b/examples/RCOverride/pid.py deleted file mode 100644 index d703fa8..0000000 --- a/examples/RCOverride/pid.py +++ /dev/null @@ -1,42 +0,0 @@ -class PID: - """PID Controller - - P: Proportional gain, tuning parameter - I: Integral gain, tuning parameter - D: Derivative gain, tuning parameter - dt: Time interval per output - - """ - - def __init__(self, P=3, I=1, D=1, dt=0.5): - self.Kp = P - self.Ki = I - self.Kd = D - - self.dt = dt - - self.clear() - - - def clear(self): - """Clears state variables.""" - self.previous_error = 0 - self.current_derivative = 0 - self.integral = 0 - - - def update(self, error): - """Updates state variables based on errors""" - self.integral = self.integral + error * self.dt - self.current_derivative = (error - self.previous_error) / self.dt - self.previous_error = error - - - def output(self, error): - """Outputs control loop feedback""" - self.update(error) - output = (self.Kp * error + - self.Ki * self.integral + - self.Kd * self.current_derivative) - - return output diff --git a/examples/RCOverride/rc_override.py b/examples/RCOverride/rc_override.py deleted file mode 100644 index 61e5eec..0000000 --- a/examples/RCOverride/rc_override.py +++ /dev/null @@ -1,206 +0,0 @@ -''' -Module for Navigation with RC-override -move_to_target() to move to a target with rc_override - -''' - -import math -import time - -import dronekit - -from pid import PID - - -def arm(vehicle): - """Arms vehicle. Taken from dronekit example""" - - print("Basic pre-arm checks") - # Don't let the user try to arm until autopilot is ready - # If you need to disable the arming check, - # just comment it with your own responsibility. - while not vehicle.is_armable: - print(" Waiting for vehicle to initialise...") - time.sleep(1) - - print("Arming motors") - vehicle.mode = dronekit.VehicleMode("MANUAL") - vehicle.armed = True - - -def move_to_target(vehicle, target_waypoint, dt=0.5): - """ - Moves vehicle to the target coordinate. Must be interrupted if 'vehicle' - only needs to move in the direction of target coordinate. - - Parameters - ---------- - vehicle : dronekit.Vehicle - Vehicle object from dronekit module. - target : dronekit.LocationGlobal - A global location object from dronekit module. - dt : float - Interval to send rc override and check if target is reached. - - Returns - ------- - reached_target : bool - True if target is reached. - - """ - - reached_target = False - while not reached_target: - time.sleep(dt) - pid_controller = PID() - correct_thrust(vehicle) - correct_yaw(vehicle, target_waypoint, pid_controller) - - reached_target = check_reached_target(vehicle, target_waypoint) - - return reached_target - - -def correct_thrust(vehicle, target_relative_altitude=50): - """Corrects thrust so vehicle stays near target relative altitude.""" - if vehicle.location.global_relative_frame.alt <= target_relative_altitude: - change_thrust(vehicle, 1700) - else: - change_thrust(vehicle, 1500) - - -def correct_yaw(vehicle, target_waypoint, pid_controller=PID()): - """Corrects the yaw so that vehicle heads towards target_waypoint""" - calculated_yaw = get_calculated_yaw(vehicle, target_waypoint, pid_controller) - change_yaw(vehicle, calculated_yaw) - - -def get_calculated_yaw(vehicle, target_waypoint, pid_controller): - """Gets the yaw to correct trajectory. - - 1000 is the value for counter-clockwise yaw (Full left stick) - 1500 is the value for hold current yaw (Mid stick). - 2000 is the value for clockwise yaw (Full right stick). - """ - neutral_yaw = 1500 - max_yaw_increment = 500 - - # calculate error btw heading and line to destination - current_waypoint = get_current_location(vehicle) - destination_angle_from_north = to_positive_angle(get_angle(current_waypoint, target_waypoint)) - heading_angle_from_north = get_heading_angle(vehicle) - - angle_difference = get_smallest_angle_diff(heading_angle_from_north, destination_angle_from_north) - error_percent = pid_controller.output(angle_difference) / 360 - increment = math.erf(error_percent) * max_yaw_increment - - calculated_yaw = round(neutral_yaw + increment) - - return calculated_yaw - - -def get_current_location(vehicle): - """Gets current lat, lon from GPS. - - Need to replace implementation of this function for true GPS denied. - """ - - return vehicle.location.global_frame - - -def get_angle(current_waypoint, target_waypoint): - """Gets angle vehicle needs to face.""" - lat_dist = target_waypoint.lat - current_waypoint.lat - lon_dist = target_waypoint.lon - current_waypoint.lon - - return math.degrees(math.atan2(lon_dist, lat_dist)) - - -def get_heading_angle(vehicle): - """Gets heading of vehicle.""" - return vehicle.heading - - -def get_distance(p0, p1): - """Gets distance between two points.""" - return math.sqrt((p0.lat - p1.lat)**2 + (p0.lon - p1.lon)**2) - - -def to_positive_angle(angle): - """Gets the positive angle.""" - return angle % 360 - - -def get_smallest_angle_diff(source, target): - """Gets the smallest difference between two angles. - - Gives a signed angle. - """ - - result = target - source - result = (result + 180) % 360 - 180 - - return result - - -def circle_currrent_location(vehicle): - """Circle location in GPS-denied. - - Don't use this function. Use LOITER instead. - """ - - change_thrust(vehicle, 1050) - vehicle.mode = dronekit.VehicleMode("CIRCLE") - - -def check_reached_target(vehicle, target_waypoint, margin_of_error=0.0001): - """Checks if vehicle has reached target. - - Margin of error is in terms of degrees. - 1 degree = 69 mi = 111045 m. - """ - - current_waypoint = get_current_location(vehicle) - distance = get_distance(target_waypoint, current_waypoint) - - if abs(distance) < margin_of_error: - return True - - return False - - -def change_yaw(vehicle, yaw): - """Sends RC override for yaw. Assumes yaw uses RC channel 4.""" - print("RC override yaw:", yaw) - vehicle.channels.overrides['4'] = yaw - - -def change_thrust(vehicle, thrust): - """Sends RC override for thrust. Assumes thrust uses RC channel 3.""" - print("RC override thrust: ", thrust) - vehicle.channels.overrides['3'] = thrust - - -# Quadrant 1 -TARGET_WAYPOINT = dronekit.LocationGlobal(-35.356833, 149.162703, None) - -# Quadrant 2 -#TARGET_WAYPOINT = dronekit.LocationGlobal(-35.359124, 149.168475, None) - -# Quadrant 3 -#TARGET_WAYPOINT = dronekit.LocationGlobal(-35.371669, 149.169827, None) - -# Quadrant 4 -#TARGET_WAYPOINT = dronekit.LocationGlobal(-35.371415, 149.162192, None) - -def main(): - vehicle = dronekit.connect('tcp:127.0.0.1:5763', wait_ready=True) - arm(vehicle) - move_to_target(vehicle, TARGET_WAYPOINT) - vehicle.mode = dronekit.VehicleMode("LOITER") - - # Sleeping so all MAVLINK messages are correctly sent - time.sleep(3) - -if __name__ == '__main__': - main() diff --git a/examples/initialize_map.py b/examples/initialize_map.py deleted file mode 100644 index 06a82e4..0000000 --- a/examples/initialize_map.py +++ /dev/null @@ -1,30 +0,0 @@ -'''Outline for processing feature points''' - -from pickle import dump, HIGHEST_PROTOCOL -import cv2 - -def main(): - '''demo feature gathering''' - scale = 0.5 - - orb = cv2.ORB_create(nfeatures=100000, scoreType=cv2.ORB_FAST_SCORE) - - area_map = cv2.imread("images/stitch_flip.jpg") - area_map = cv2.resize(area_map, (0, 0), fx=scale, fy=scale) - area_map = cv2.cvtColor(area_map, cv2.COLOR_BGR2GRAY) - - # find the keypoints and descriptors with ORB - keys, descs = orb.detectAndCompute(area_map, None) - length, width = area_map.shape - - with open('map.bin', 'wb') as output: # Overwrites any existing file. - obj = (map(lambda x: {'pt': x.pt, 'size': x.size, 'angle': x.angle, \ - 'response': x.response, 'octave': x.octave, 'class_id': x.class_id}, keys)) - dump({'length': length, 'width': width, 'descs': descs, 'keys': obj}, \ - output, HIGHEST_PROTOCOL) - - print('dumped keys and descs to \'map.bin\'') - output.close() - -if __name__ == "__main__": - main() diff --git a/examples/take_pictures.py b/examples/take_pictures.py deleted file mode 100644 index c78606d..0000000 --- a/examples/take_pictures.py +++ /dev/null @@ -1,23 +0,0 @@ -'''The purpose of this script is to have the PiCamera automatically take photos -when the Raspberry Pi turns on at the start of a mission.''' - -from picamera import PiCamera #pylint: disable=import-error - - -def main(): - '''demo picture taking on rpi''' - camera = PiCamera() - camera.iso = 150 # Standard ISO values are 100/200 for similar aperature - camera.shutter_speed = camera.exposure_speed - gain = camera.awb_gains - camera.exposure_mode = 'off' - camera.awb_gains = gain - camera.resolution = (1024, 768) - i = 0 # counter for the images - while True: - camera.capture('/home/pi/camtestdir/image_%s.jpg' % i) - i += 1 - - -if __name__ == "__main__": - main() diff --git a/sim.py b/sim.py deleted file mode 100644 index d1ac047..0000000 --- a/sim.py +++ /dev/null @@ -1,3 +0,0 @@ -import subprocess - -subprocess.call('simulator/run.sh') diff --git a/simulation_images/0.png b/simulation_images/0.png deleted file mode 100644 index 5811213..0000000 Binary files a/simulation_images/0.png and /dev/null differ diff --git a/simulation_images/1.jpg b/simulation_images/1.jpg deleted file mode 100644 index 209ae31..0000000 Binary files a/simulation_images/1.jpg and /dev/null differ diff --git a/simulation_images/10.jpg b/simulation_images/10.jpg deleted file mode 100644 index 6381644..0000000 Binary files a/simulation_images/10.jpg and /dev/null differ diff --git a/simulation_images/11.jpg b/simulation_images/11.jpg deleted file mode 100644 index 1153ae4..0000000 Binary files a/simulation_images/11.jpg and /dev/null differ diff --git a/simulation_images/12.jpg b/simulation_images/12.jpg deleted file mode 100644 index 39fb500..0000000 Binary files a/simulation_images/12.jpg and /dev/null differ diff --git a/simulation_images/13.jpg b/simulation_images/13.jpg deleted file mode 100644 index ac70b05..0000000 Binary files a/simulation_images/13.jpg and /dev/null differ diff --git a/simulation_images/2.jpg b/simulation_images/2.jpg deleted file mode 100644 index a423674..0000000 Binary files a/simulation_images/2.jpg and /dev/null differ diff --git a/simulation_images/3.jpg b/simulation_images/3.jpg deleted file mode 100644 index 77b5ffb..0000000 Binary files a/simulation_images/3.jpg and /dev/null differ diff --git a/simulation_images/4.jpg b/simulation_images/4.jpg deleted file mode 100644 index ce962bf..0000000 Binary files a/simulation_images/4.jpg and /dev/null differ diff --git a/simulation_images/5.jpg b/simulation_images/5.jpg deleted file mode 100644 index f9cef90..0000000 Binary files a/simulation_images/5.jpg and /dev/null differ diff --git a/simulation_images/6.jpg b/simulation_images/6.jpg deleted file mode 100644 index 2154d6f..0000000 Binary files a/simulation_images/6.jpg and /dev/null differ diff --git a/simulation_images/7.jpg b/simulation_images/7.jpg deleted file mode 100644 index 6839522..0000000 Binary files a/simulation_images/7.jpg and /dev/null differ diff --git a/simulation_images/8.jpg b/simulation_images/8.jpg deleted file mode 100644 index 7b6f9d2..0000000 Binary files a/simulation_images/8.jpg and /dev/null differ diff --git a/simulation_images/9.jpg b/simulation_images/9.jpg deleted file mode 100644 index 74545cc..0000000 Binary files a/simulation_images/9.jpg and /dev/null differ diff --git a/simulator/sim.py b/simulator/sim.py new file mode 100644 index 0000000..7d5dcae --- /dev/null +++ b/simulator/sim.py @@ -0,0 +1,23 @@ +'''Runs simulator using python''' + +import os +from time import sleep +from dronekit_sitl import SITL +SIM = {} +BIN = os.getenv("SITL_BIN") +DO_DOWNLOAD = True +if BIN is not None: + DO_DOWNLOAD = False + SIM["path"] = BIN + DEFAULTS = os.getenv("SITL_DEFAULTS_FILEPATH") + if DEFAULTS is not None: + SIM["defaults_filepath"] = DEFAULTS +SIM = SITL(**SIM) +if DO_DOWNLOAD: + SIM.download('copter', '3.3', verbose=False) + +SIM.launch(['--model', 'hex', ], await_ready=True, restart=False) +print("Simulator lanched") + +while True: + sleep(5) diff --git a/src/coms.py b/src/coms.py index 1d0dc1b..430233f 100644 --- a/src/coms.py +++ b/src/coms.py @@ -1,58 +1,11 @@ -'''methods neccecary for coms''' +"""methods neccecary for coms""" import time -from threading import Lock, Thread -import sys -import subprocess import json -import msgpack -from digi.xbee.devices import XBeeDevice, RemoteXBeeDevice, XBee64BitAddress -class ComsMutex: - '''maintains order between threads''' - def __init__(self): - self.start_mutex = Lock() - self.start = False - - self.stop_mutex = Lock() - self.stop = False - - self.xbee_mutex = Lock() - self.xbee = None - - self.acknowledgement_mutex = Lock() - self.acknowledgement_mutex = False - - -def mac_xbee_port_name(): - '''Looks in /dev directory for connected XBee serial port name on a macOS.''' - try: - # System call to get port name of connected XBee radio - port_name = subprocess.check_output(["ls", "/dev/"]) - - # index in dev directory of port name - i = port_name.index("tty.usbserial-") - # 22 is length of "tty.usbserial-" + 8-char port name - return "/dev/" + port_name[i: i + 22] - - except ValueError: - raise ValueError("Value Error: \'tty.usbserial-\' not found in /dev") - - -class DummyMessage: - '''Dummy msg class for comm simulation thread to be compatible with xbee_callback function''' - def __init__(self, data=None): - self.data = data # UTF-8 encoded JSON message - self.remote_device = DummyRemoteDevice() - - -class DummyRemoteDevice: - '''Dummy remote device object for use in DummyMessage''' - def __init__(self): - self.get_64bit_addr = lambda _: 'comms simulation' +class Coms: + """compartenmentalizes coms functionality and scope""" -class Coms(): - '''compartenmentalizes coms functionality and scope''' configs = None con_timestamp = 0 gcs_timestamp = 0 @@ -62,123 +15,40 @@ class Coms(): xbee_callback = None def __init__(self, configs, xbee_callback): - '''initializes coms object''' + """initializes coms object""" self.configs = configs self.xbee_callback = xbee_callback - self.mutex = ComsMutex() - - if configs['coms_simulated'] is True: - sim_file = configs["comm_sim_file"] - comm_sim = Thread(target=self.comm_simulation, args=(sim_file,)) - comm_sim.start() - else: - try: - port_name = "" - if sys.platform == "darwin": - port_name = mac_xbee_port_name() - elif sys.platform == "linux" or sys.platform == "linux2": - port_name = "/dev/ttyUSB0" - # TODO: figure out windows port name - elif sys.platform == "win32": - port_name = "COMS1" - - # Instantiate XBee device object. - self.xbee = XBeeDevice(port_name, 57600) - self.xbee.open() - - # If error in setting up XBee, try again - except TimeoutError as ex: - print(ex) - print("Connect the XBee radio!") - time.sleep(5) - - - def send_till_ack(self, address, msg, msg_id): - '''Continuously sends message to given address until acknowledgement - message is recieved with the corresponding ackid.''' - # Instantiate a remote XBee device object to send data. - packed_data = bytearray(msgpack.packb(msg)) - while self.ack_id != msg_id: - self.send(address, packed_data) - time.sleep(1) - - def acknowledge(self, address, ack_id): - '''Sends message received acknowledgement to GCS - param address: address of GCS''' - ack = { - "type": "ack", - "time": round(time.clock() - self.con_timestamp) + self.gcs_timestamp, - "sid": self.configs['vehicle_id'], - "tid": 0, # The ID of GCS - "id": self.new_msg_id(), - "ackid": ack_id - } - self.send(address, ack) + sim_file = configs["comm_sim_file"] + self.comm_simulation(sim_file) - - def new_msg_id(self): - '''Increments msg_id and returns unique id for new message''' - self.msg_id += 1 - return self.msg_id - - - def recieve(self): - '''Instantiate XBee device''' - try: - self.xbee.flush_queues() - print("Waiting for data...\n") - while True: - xbee_message = self.xbee.read_data() - if xbee_message is not None: - print("Received '%s' from %s" % (xbee_message.data.decode(), \ - xbee_message.remote_self.xbee.get_64bit_addr())) - finally: - if self.xbee is not None and self.xbee.is_open(): - self.xbee.close() - - - def send(self, mac_address, data): - '''sends data over xbee''' - remote_device = RemoteXBeeDevice(self.xbee, XBee64BitAddress.from_hex_string(mac_address)) - - if remote_device is None: - print("Invalid MAC Address") - return - - print("Sending '%s' to %s" % (data, remote_device.get_64bit_addr())) - self.mutex.xbee_mutex.acquire() - self.xbee.send_data(remote_device, data) - self.mutex.xbee_mutex.release() - print("Success") - - - def bad_msg(self, address, problem): - '''Sends "bad message" to GCS if message received was poorly formatted/unreadable - and describes error from parsing original message. - :param address: address of GCS - :param problem: string describing error from parsing original message''' - msg = { - "type": "badMessage", - "time": round(time.clock() - self.con_timestamp) + self.gcs_timestamp, - "sid": self.configs['vehicle_id'], - "tid": 0, # The ID of GCS - "id": self.new_msg_id(), - - "error": problem - } - self.send(address, msg) - - # TODO: needs to be updated def comm_simulation(self, comm_file): - '''Reads through comm simulation file from configs and calls - xbee_callback to simulate radio messages.''' + """Reads through comm simulation file from configs and calls + xbee_callback to simulate radio messages.""" + print("listening for messages") with open(comm_file, "r") as com_data: comms = json.load(com_data) # reads the json file prev_time = 0 - for instr in comms: # gets time and message from each json object (instruction) + for ( + instr + ) in comms: # gets time and message from each json object (instruction) curr_time = instr["time"] time.sleep(curr_time - prev_time) # waits for the next instruction # Send message to xbee_callback self.xbee_callback(DummyMessage(json.dumps(instr["message"]))) prev_time = curr_time + + +class DummyMessage: + """Dummy msg class for comm simulation thread to be compatible with xbee_callback function""" + + def __init__(self, data=None): + self.data = data # UTF-8 encoded JSON message + self.remote_device = DummyRemoteDevice() + + +class DummyRemoteDevice: + """Dummy remote device object for use in DummyMessage""" + + def __init__(self): + self.get_64bit_addr = lambda _: "comms simulation" diff --git a/src/conversions.py b/src/conversions.py index 537b204..d842933 100755 --- a/src/conversions.py +++ b/src/conversions.py @@ -1,4 +1,4 @@ -#pylint: disable=invalid-name +# pylint: disable=invalid-name """ Code from https://github.com/scivision/pymap3d @@ -7,42 +7,47 @@ from numpy import radians, sin, cos, tan, arctan, hypot, degrees, arctan2, sqrt, pi import numpy as np + class Ellipsoid: """ generate reference ellipsoid parameters https://en.wikibooks.org/wiki/PROJ.4#Spheroid """ - def __init__(self, model='wgs84'): - if model == 'wgs84': - #https://en.wikipedia.org/wiki/World_Geodetic_System#WGS84 - self.a = 6378137. # semi-major axis [m] + def __init__(self, model="wgs84"): + if model == "wgs84": + # https://en.wikipedia.org/wiki/World_Geodetic_System#WGS84 + self.a = 6378137.0 # semi-major axis [m] self.f = 1 / 298.2572235630 # flattening self.b = self.a * (1 - self.f) # semi-minor axis - elif model == 'grs80': - #https://en.wikipedia.org/wiki/GRS_80 - self.a = 6378137. # semi-major axis [m] + elif model == "grs80": + # https://en.wikipedia.org/wiki/GRS_80 + self.a = 6378137.0 # semi-major axis [m] self.f = 1 / 298.257222100882711243 # flattening self.b = self.a * (1 - self.f) # semi-minor axis - elif model == 'clrk66': # Clarke 1866 + elif model == "clrk66": # Clarke 1866 self.a = 6378206.4 # semi-major axis [m] self.b = 6356583.8 # semi-minor axis self.f = -(self.b / self.a - 1) - elif model == 'mars': # https://tharsis.gsfc.nasa.gov/geodesy.html + elif model == "mars": # https://tharsis.gsfc.nasa.gov/geodesy.html self.a = 3396900 self.b = 3376097.80585952 self.f = 1 / 163.295274386012 - elif model == 'moon': - self.a = 1738000. - self.b = 1738000. - self.f = 0. - elif model == 'venus': - self.a = 6051000. - self.b = 6051000. - self.f = 0. + elif model == "moon": + self.a = 1738000.0 + self.b = 1738000.0 + self.f = 0.0 + elif model == "venus": + self.a = 6051000.0 + self.b = 6051000.0 + self.f = 0.0 else: - raise NotImplementedError('''{} model not implemented, let us know - and we will add it (or make a pull request)'''.format(model)) + raise NotImplementedError( + """{} model not implemented, let us know + and we will add it (or make a pull request)""".format( + model + ) + ) def get_radius_normal(lat_radians, ell): @@ -53,7 +58,9 @@ def get_radius_normal(lat_radians, ell): a = ell.a b = ell.b - return a**2 / sqrt(a**2 * cos(lat_radians)**2 + b**2 * sin(lat_radians)**2) + return a ** 2 / sqrt( + a ** 2 * cos(lat_radians) ** 2 + b ** 2 * sin(lat_radians) ** 2 + ) def ecef2ned(x, y, z, lat0, lon0, h0, ell=None, deg=True): @@ -72,6 +79,7 @@ def ecef2ned(x, y, z, lat0, lon0, h0, ell=None, deg=True): return n, e, -u + def ecef2enu(x, y, z, lat0, lon0, h0, ell, deg): """ input @@ -90,9 +98,9 @@ def ecef2enu(x, y, z, lat0, lon0, h0, ell, deg): def uvw2enu(u, v, w, lat0, lon0, deg=True): - ''' + """ uvw to enu - ''' + """ if deg: lat0 = radians(lat0) lon0 = radians(lon0) @@ -123,30 +131,31 @@ def geodetic2ecef(lat, lon, alt, ell=None, deg=True): lat = radians(lat) lon = radians(lon) - with np.errstate(invalid='ignore'): + with np.errstate(invalid="ignore"): # need np.any() to handle scalar and array cases if np.any((lat < -pi / 2) | (lat > pi / 2)): - raise ValueError('-90 <= lat <= 90') + raise ValueError("-90 <= lat <= 90") if np.any((lon < -pi) | (lon > 2 * pi)): - raise ValueError('-180 <= lat <= 360') + raise ValueError("-180 <= lat <= 360") if np.any(np.asarray(alt) < 0): - raise ValueError('altitude [0, Infinity)') + raise ValueError("altitude [0, Infinity)") # radius of curvature of the prime vertical section N = get_radius_normal(lat, ell) # Compute cartesian (geocentric) coordinates given (curvilinear) geodetic # coordinates. x = (N + alt) * cos(lat) * cos(lon) y = (N + alt) * cos(lat) * sin(lon) - z = (N * (ell.b / ell.a)**2 + alt) * sin(lat) + z = (N * (ell.b / ell.a) ** 2 + alt) * sin(lat) return x, y, z + def enu2uvw(east, north, up, lat0, lon0, deg=True): - ''' + """ enu to uvw - ''' + """ if deg: lat0 = radians(lat0) lon0 = radians(lon0) @@ -159,6 +168,7 @@ def enu2uvw(east, north, up, lat0, lon0, deg=True): return u, v, w + def enu2ecef(e1, n1, u1, lat0, lon0, h0, deg, ell=None): """ ENU to ECEF @@ -176,6 +186,7 @@ def enu2ecef(e1, n1, u1, lat0, lon0, h0, deg, ell=None): return x0 + dx, y0 + dy, z0 + dz + def ned2geodetic(n, e, d, lat0, lon0, h0, ell=None, deg=True): """ input @@ -212,12 +223,15 @@ def ecef2geodetic(x, y, z, ell=None, deg=True): if ell is None: ell = Ellipsoid() - r = sqrt(x**2 + y**2 + z**2) + r = sqrt(x ** 2 + y ** 2 + z ** 2) - E = sqrt(ell.a**2 - ell.b**2) + E = sqrt(ell.a ** 2 - ell.b ** 2) # eqn. 4a - u = sqrt(0.5 * (r**2 - E**2) + 0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2)) + u = sqrt( + 0.5 * (r ** 2 - E ** 2) + + 0.5 * sqrt((r ** 2 - E ** 2) ** 2 + 4 * E ** 2 * z ** 2) + ) Q = hypot(x, y) @@ -227,17 +241,18 @@ def ecef2geodetic(x, y, z, ell=None, deg=True): Beta = arctan(huE / u * z / hypot(x, y)) # eqn. 13 - eps = ((ell.b * u - ell.a * huE + E**2) * sin(Beta)) / \ - (ell.a * huE * 1 / cos(Beta) - E**2 * cos(Beta)) + eps = ((ell.b * u - ell.a * huE + E ** 2) * sin(Beta)) / ( + ell.a * huE * 1 / cos(Beta) - E ** 2 * cos(Beta) + ) Beta += eps -# %% final output + # %% final output lat = arctan(ell.a / ell.b * tan(Beta)) lon = arctan2(y, x) # eqn. 7 - alt = sqrt((z - ell.b * sin(Beta))**2 + (Q - ell.a * cos(Beta))**2) + alt = sqrt((z - ell.b * sin(Beta)) ** 2 + (Q - ell.a * cos(Beta)) ** 2) if deg: return degrees(lat), degrees(lon), alt diff --git a/src/cv.py b/src/cv.py deleted file mode 100644 index 1852c43..0000000 --- a/src/cv.py +++ /dev/null @@ -1,58 +0,0 @@ -''' -cv simulation and module for VTOL -''' - -import re -from os import listdir, environ -from threading import Thread -import time -import subprocess -import cv2 - -class Vision(): - ''' vision class for managing cv ''' - image_counter = 0 - - @classmethod - def sorted_alphanumeric(cls, data): - ''' sorts data alphanumerically''' - convert = lambda text: int(text) if text.isdigit() else text.lower() - alphanum_key = lambda key: [convert(c) for c in re.split('([0-9]+)', key)] - return sorted(data, key=alphanum_key) - - def cv_simulation(self, configs): - '''simulation for VTOL cv''' - files = self.sorted_alphanumeric(listdir(configs["cv_simulated"]["directory"])) - path = configs["cv_simulated"]["directory"] + "/" + files[self.image_counter % len(files)] - img = cv2.imread(path, 1) - self.image_counter += 1 - return img - - @classmethod - def init_camera(cls, configs): - '''initialize camera for 3DR Solo''' - if configs["3dr_solo"]: - solo_connect_thread = Thread(target=lambda _: \ - subprocess.check_output(["nc", "10.1.1.1", "5502"])) - solo_connect_thread.daemon = True - solo_connect_thread.start() - time.sleep(1) - environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp' - cam = cv2.VideoCapture("./sololink.sdp") - return cam - # initialize camera for Raspberry PI - from picamera import PiCamera #pylint: disable=import-error,import-outside-toplevel - cam = PiCamera() #pylint: disable=import-error - return cam - - - @classmethod - def take_picture(cls, camera, configs): - '''takes picture using config''' - if configs["3dr_solo"]: - return camera.read() - from picamera.array import PiRGBArray #pylint: disable=import-error,import-outside-toplevel - raw_capture = PiRGBArray(camera) #pylint: disable=import-error - camera.capture(raw_capture, format="bgr") - return raw_capture.array - \ No newline at end of file diff --git a/src/gps_denied_config.json b/src/gps_denied_config.json deleted file mode 100644 index 9cd1819..0000000 --- a/src/gps_denied_config.json +++ /dev/null @@ -1,30 +0,0 @@ -{ - "vehicle_type": "VTOL", - - "vehicle_id": 1, - "vehicle_simulated": true, - "coms_simulated": true, - "comm_sim_file": "comm_sim_example.json", - "3dr_solo": false, - "cv_simulated": { - "toggled_on": true, - "directory": "./simulation_images/" - }, - "mission_control_MAC": "0013A200409BD79C", - "baud_rate": 57600, - "altitude": 10, - "air_speed": 10, - "waypoint_tolerance": 3, - "motion" : - { - "P": 0.10, - "I": 0.02, - "D": 0.01 - }, - "pos": - { - "P": 0.13, - "I": 0.00005, - "D": 0.00001 - } -} \ No newline at end of file diff --git a/src/gps_denied_v2.py b/src/gps_denied_v2.py deleted file mode 100644 index b0687f5..0000000 --- a/src/gps_denied_v2.py +++ /dev/null @@ -1,219 +0,0 @@ -''' -First thoughts on SLAM cv -''' -import ssl -import os -# from threading import Thread -import json -import urllib.request -import numpy as np -import cv2 -from simple_pid import PID -from shapely.geometry import Polygon, Point -from dronekit import connect -from dotenv import load_dotenv -from vtol import VTOL - -load_dotenv() - -def setup_vehicle(configs): - '''sets up gps denied vehicle''' - con_str = "tcp:127.0.0.1:5763" - veh = connect(con_str, wait_ready=True, vehicle_class=GpsDeniedVtol) - veh.configs = configs - # veh.setup_coms() - return veh - - -class GpsDeniedVtol(VTOL): #pylint: disable=too-many-instance-attributes - '''encapsualtes variables and tools used for GPS denied navigation''' - - # Quick configs - total_tilt = 25 - target_position = (0, 0) - reached_dest = False - - locating = True - - #CV sim - frame_size = (600, 280) - misses = 0 - position_guess = (0, 0) - - def get_image(self): - '''gets image at vehicle's current lat lng position''' - lat = self.location.global_relative_frame.lat - lng = self.location.global_relative_frame.lon - #Gets image of current lat long coorindates from google static maps - url = 'https://maps.googleapis.com/maps/api/staticmap?center=' + \ - '{},{}&zoom=20&size=600x300&maptype=satellite&key={}' \ - .format(lat, lng, os.getenv('google_maps_api_key')) - context = ssl._create_unverified_context() # pylint: disable=protected-access - resp = urllib.request.urlopen(url, context=context) - image = np.asarray(bytearray(resp.read()), dtype="uint8") - decoded = cv2.imdecode(image, cv2.IMREAD_GRAYSCALE) - cropped = decoded[:280, :] - return cropped - - - def set_position(self): - '''Uses PID and position guesses to set the quad's position''' - assert self.target_position is not None - - #set attitude to 0 - self.set_attitude(yaw_angle=0, duration=5) - while True: - print("Targeting", self.target_position) - targ = [] - targ = self.target_position - xpid = PID(self.configs.pos.P, self.configs.pos.I, self.configs.pos.D, setpoint=targ[0]) - xpid.sample_time = 0.33 - ypid = PID(self.configs.pos.P, self.configs.pos.I, self.configs.pos.D, setpoint=targ[1]) - ypid.sample_time = 0.33 - till_stop = 15 - while True: - if targ != self.target_position: - #if dest changes rest PID algs - break - if till_stop == 0: - self.reached_dest = True - #Get control from each PID - x_control = xpid(self.position_guess[0]) - y_control = ypid(self.position_guess[1]) - remaining_x = targ[0] - self.position_guess[0] - remaining_y = targ[1] - self.position_guess[1] - remaining_dst_sq = (remaining_x ** 2 + remaining_y ** 2) ** .5 - if remaining_dst_sq < 20: - till_stop -= 1 - else: - till_stop = 15 - #Correct for tilts higher than total_tilt - total_control_sq = (y_control ** 2 + x_control ** 2) ** .5 - if total_control_sq > self.total_tilt: - x_control = self.total_tilt * remaining_x / remaining_dst_sq - y_control = self.total_tilt * remaining_y / remaining_dst_sq - - self.set_attitude(roll_angle=x_control, \ - pitch_angle=y_control, \ - duration=.33, \ - yaw_angle=0) - - - def cv_stitch(self): - '''Gathers and organizes feature points. Reports position guesses''' - point_img = np.zeros([5000, 5000]) - - min_match_count = 10 - # Initiate SIFT detector - - orb = cv2.ORB_create(nfeatures=1000, scoreType=cv2.ORB_FAST_SCORE) - - flann_index_lsh = 6 - index_params = dict( - algorithm=flann_index_lsh, - table_number=6, # 12 - key_size=12, # 20 - multi_probe_level=1, - ) # 2 - - search_params = dict(checks=5) - flann = cv2.FlannBasedMatcher(index_params, search_params) - - base_image = self.get_image() - - map_kp, map_des = orb.detectAndCompute(base_image, None) - - dim = base_image.shape - low = .1 - high = 1 - low - image_bounds = [(dim[1] * low, dim[0] * low), (dim[1] * high, dim[0] * low), \ - (dim[1] * high, dim[0] * high), (dim[1] * low, dim[0] * high)] - seen_poly = Polygon() - - while self.locating: - # fetch key points from google image - img = self.get_image() - cur_kp, cur_des = orb.detectAndCompute(img, None) - # gathers map keys within frame size - if self.misses < 3: - keys_within_region = [i for i, v in enumerate(map_kp) if \ - v.pt[0] >= self.position_guess[0] - self.frame_size[0] / 2 and \ - v.pt[0] <= self.position_guess[0] + self.frame_size[0] * 1.5 and \ - v.pt[1] >= self.position_guess[1] - self.frame_size[1] / 2 and \ - v.pt[1] <= self.position_guess[1] + self.frame_size[1] * 1.5] - #Use map des within the bounding box - relevant_map_des = np.array([map_des[i] for i in keys_within_region]) - relevant_map_kp = [map_kp[i] for i in keys_within_region] - else: - #Use all map des - relevant_map_des = map_des - relevant_map_kp = map_kp - matches = flann.knnMatch(relevant_map_des, cur_des, k=2) - # store all the good matches as per Lowe's ratio test. - good = [] - try: - for match_a, match_b in matches: - if match_a.distance < 0.7 * match_b.distance: - good.append(match_a) - except ValueError: - print('err') - - if len(good) > min_match_count: - self.misses = 0 - #pylint: disable=too-many-function-args - src_pts = np.float32([cur_kp[m.trainIdx].pt for m in good]) \ - .reshape(-1, 1, 2) - #pylint: disable=too-many-function-args - dst = np.float32([relevant_map_kp[m.queryIdx].pt for m in good]) \ - .reshape(-1, 1, 2) - - m_matrix, _ = cv2.findHomography(src_pts, dst, cv2.LMEDS, 5.0) - - im_points = cv2.drawKeypoints(img, cur_kp, None) - cv2.imshow('test', im_points) - cv2.waitKey(5) - - # translate points by m matrix - for key in cur_kp: - key.pt = tuple(np.matmul(m_matrix, key.pt + (1,))[:2]) - - # estimae new position - new_guess = np.matmul(m_matrix, [img.shape[1] / 2, img.shape[0] / 2, 1.0])[:2] - - # find points outside of already scanned region and within bounds - bounds = Polygon(list(map(lambda bound: np.matmul(m_matrix, bound + \ - (1.0,))[:2], image_bounds))) - new_pts = [i for i, v in enumerate(cur_kp) if bounds.contains(Point(v.pt)) \ - and not seen_poly.contains(Point(v.pt))] - - print("Found ", len(new_pts), new_guess) - self.position_guess = new_guess - - if seen_poly.exterior is None or seen_poly.exterior.distance(Point(new_guess)) \ - < 100 and new_pts: - print('added new') - new_des = [cur_des[i] for i in new_pts] #pylint: disable=unused-variable - new_kp = [cur_kp[i] for i in new_pts] #pylint: disable=unused-variable - - for key in new_kp: - cv2.circle(point_img, (int(key.pt[0]), int(key.pt[1])), \ - 10, 255, thickness=10) - - cv2.imshow('points', cv2.resize(point_img, (500, 500))) - - seen_poly = seen_poly.union(Polygon(bounds)) - - # map_kp.extend(new_kp) - # map_des = np.append(map_des, new_des, axis=0) - - map_kp = cur_kp - map_des = cur_des - else: - print("Not enough matches are found - %d/%d" % (len(good), min_match_count)) - self.misses += 1 - cv2.waitKey(5) - -with open('configs.json', 'r') as data: - VEH = setup_vehicle(json.load(data)) - VEH.takeoff() - VEH.cv_stitch() diff --git a/src/hex.py b/src/hex.py new file mode 100644 index 0000000..a5c6a6b --- /dev/null +++ b/src/hex.py @@ -0,0 +1,85 @@ +"""Automous tools for HEX""" +import time +import json +from dronekit import VehicleMode, Vehicle, LocationGlobalRelative +from coms import Coms +from util import get_distance_metres + + +class HEX(Vehicle): + """ HEX basic state isolated""" + + coms = None + + def __init__(self, configs, *args): # pylint: disable=useless-super-delegation + super().__init__(*args) + self.configs = configs + + def setup(self): + """initializes coms""" + self.coms = Coms(self.configs, self.coms_callback) + + # pylint: disable=no-self-use + def coms_callback(self, message): + """callback for radio messages""" + parsed_message = json.loads(message.data) + # gives us the specific command we want the drone to executre + command = parsed_message["type"] + print("recieved message {}".format(command)) + + # START HERE + + # Respond to takeoff command + + # Respond to go_to command + + # Respond to land command + + def takeoff(self): + """Commands drone to take off by arming vehicle and flying to altitude""" + print("Pre-arm checks") + while not self.is_armable: + print("Waiting for vehicle to initialize") + time.sleep(1) + + print("Arming motors") + self.mode = VehicleMode("GUIDED") + self.armed = True + + while not self.armed: + print("Waiting to arm vehicle") + time.sleep(1) + + print("Taking off") + + altitude = self.configs["altitude"] + self.simple_takeoff(altitude) # take off to altitude + + # Wait until vehicle reaches minimum altitude + while self.location.global_relative_frame.alt < altitude * 0.95: + print("Altitude: " + str(self.location.global_relative_frame.alt)) + time.sleep(1) + + print("Reached target altitude") + + def go_to(self, lat, lon): + """Commands drone to fly to a specified point perform a simple_goto """ + point = LocationGlobalRelative(lat, lon, self.configs["altitude"]) + self.simple_goto(point, self.configs["air_speed"]) + + while True: + distance = get_distance_metres(self.location.global_relative_frame, point) + if distance > self.configs["waypoint_tolerance"]: + print("Distance remaining:", distance) + time.sleep(1) + else: + break + print("Target reached") + + def land(self): + """Commands vehicle to land""" + # Implement land + # See above methods as examples + # https://dronekit-python.readthedocs.io/en/latest/guide/copter/guided_mode.html + # https://dronekit-python.readthedocs.io/en/latest/examples/guided-set-speed-yaw-demo.html + self.simple_takeoff(0) diff --git a/src/main.py b/src/main.py index bdcc47a..fb813e6 100644 --- a/src/main.py +++ b/src/main.py @@ -1,27 +1,17 @@ -'''main executable for setting up VTOL''' +"""main executable for setting up HEX""" import json from util import setup_vehicle -from vtol import VTOL +from hex import HEX -def main(configs): - '''Configure vtol and ready for mission''' - vehicle = setup_vehicle(configs, VTOL) - vehicle.takeoff() - - vehicle.set_altitude(10) - - home = vehicle.location.global_relative_frame - - # Pick-up function for ping pong balls - vehicle.land() - - vehicle.takeoff() - - vehicle.go_to(home) +def main(configs): + """Configure hexcopter and ready for mission""" + hexa = setup_vehicle(configs, HEX) + hexa.takeoff() + hexa.go_to(27.28, 153.01) + hexa.land() - vehicle.land() -if __name__ == '__main__': - with open('configs.json', 'r') as data: +if __name__ == "__main__": + with open("configs.json", "r") as data: main(json.load(data)) diff --git a/src/quadplane.py b/src/quadplane.py deleted file mode 100644 index 9df72ee..0000000 --- a/src/quadplane.py +++ /dev/null @@ -1,18 +0,0 @@ -'''Automous tools for VTOL''' -import json -from vtol import VTOL -from util import setup_vehicle - -class QuadPlane(VTOL): - '''State deffinition for QuadPlane''' - land_mode = 'QLAND' - - def setup(self): - '''vtol specific steps needed before flight''' - print('setting params') - self.parameters['Q_GUIDED_MODE'] = 1 - -if __name__ == '__main__': - with open('configs.json', 'r') as data: - VEHICLE = setup_vehicle(json.load(data), QuadPlane) - VEHICLE.takeoff() diff --git a/src/quadplane_main.py b/src/quadplane_main.py deleted file mode 100644 index 759e0a1..0000000 --- a/src/quadplane_main.py +++ /dev/null @@ -1,23 +0,0 @@ -'''main executable for setting up VTOL''' -import json -from util import setup_vehicle -from quadplane import QuadPlane - -def main(configs): - '''Configure vtol and ready for mission''' - vehicle = setup_vehicle(configs, QuadPlane) - - vehicle.takeoff() - vehicle.set_attitude(pitch_angle=-5, duration=10, yaw_angle=180) - - # vehicle.land() - - # home = vehicle.location.global_relative_frame - - # Pick-up function for ping pong balls - - # vehicle.land() - -if __name__ == '__main__': - with open('configs.json', 'r') as data: - main(json.load(data)) diff --git a/src/tests/test_hex.py b/src/tests/test_hex.py new file mode 100644 index 0000000..a8e0524 --- /dev/null +++ b/src/tests/test_hex.py @@ -0,0 +1,35 @@ +"""test for hex.py""" +from dronekit_sitl import start_default +from util import setup_vehicle +from hex import HEX + +CONFIGS = { + "vehicle_simulated": True, + "vehicle_id": 1, + "coms_simulated": True, + "altitude": 5, + "comm_sim_file": "comm_sim_example.json", + "air_speed": 20, + "simulation": {"defaultPort": 5760, "shelveName": "shelfStore.store"}, +} + +CON_STR = start_default().connection_string() + +with open("configs.json", "r") as data: + VEHICLE = setup_vehicle(CONFIGS, HEX) + + +def test_takeoff(): + """hexcopter dronekit-sitl takeoff""" + global VEHICLE # pylint: disable=global-statement + VEHICLE.takeoff() + alt = VEHICLE.location.global_relative_frame.alt + assert 4 < alt < 6, "vehicle did not reach target alt" + + +def test_land(): + """hexcopter dronekit-sitl land""" + global VEHICLE # pylint: disable=global-statement + VEHICLE.land() + alt = VEHICLE.location.global_relative_frame.alt + assert -1 < alt < 1, "vehicle did not land" diff --git a/src/tests/test_vtol.py b/src/tests/test_vtol.py deleted file mode 100644 index db55dbe..0000000 --- a/src/tests/test_vtol.py +++ /dev/null @@ -1,48 +0,0 @@ -'''test for vtol.py''' -import pytest -from dronekit_sitl import start_default -from dronekit import connect -from vtol import VTOL - -CONFIGS = { - 'vehicle_simulated': True, - 'vehicle_id': 1, - 'coms_simulated': True, - 'altitude': 5, - 'comm_sim_file': 'comm_sim_example.json', - 'air_speed': 20, - 'simulation': { - 'defaultPort': 5760, - 'shelveName': 'shelfStore.store' - } -} - -CON_STR = start_default().connection_string() - -with open('configs.json', 'r') as data: - VEHICLE = connect(CON_STR, wait_ready=True, vehicle_class=VTOL) - VEHICLE.configs = CONFIGS - -def test_takeoff(): - '''quadcopter dronekit-sitl takeoff''' - global VEHICLE # pylint: disable=global-statement - VEHICLE.takeoff() - alt = VEHICLE.location.global_relative_frame.alt - assert 4 < alt < 6, "vehicle did not reach target alt" - - -def test_land(): - '''quadcopter dronekit-sitl land''' - global VEHICLE # pylint: disable=global-statement - VEHICLE.land() - alt = VEHICLE.location.global_relative_frame.alt - assert -1 < alt < 1, "vehicle did not land" - -def test_change_status(): - '''set status sets the vehicles status correctly and throws with invalid status''' - global VEHICLE # pylint: disable=global-statement - VEHICLE.change_status('paused') - assert VEHICLE.status == 'paused' - - with pytest.raises(Exception): - VEHICLE.change_status('invalid') diff --git a/src/util.py b/src/util.py index 92e6c38..86c7f1b 100644 --- a/src/util.py +++ b/src/util.py @@ -1,65 +1,57 @@ -'''Util methods for VTOL''' +"""Util methods for HEX""" import json import datetime import subprocess import sys from math import cos, sin, radians, sqrt import shelve +from functools import partial from dronekit import connect, APIException def setup_vehicle(configs, v_type): - '''Sets up self as a quadplane vehicle''' - if configs["vehicle_simulated"]: - veh = scan_ports(configs, v_type) - else: - if configs["SOLO"]: - con_str = "udpin:0.0.0.0:14550" - else: - # connect to pixhawk via MicroUSB - # if we switch back to using the telem2 port, use "/dev/serial0" - con_str = "/dev/ttyACM0" - veh = connect(con_str, baud=configs["baud_rate"], wait_ready=True, \ - vehicle_class=v_type, heartbeat_timeout=5, timeout=5) - veh.configs = configs - veh.airspeed = configs['air_speed'] - veh.setup() + """Sets up self as a hexcopter vehicle""" + v_type = partial(v_type, configs) + veh = scan_ports(configs, v_type) return veh def scan_ports(configs, v_type): - '''scans TCP ports to find open simulator''' + """scans TCP ports to find open simulator""" itteration = 0 - shelf = shelve.open(configs['simulation']['shelveName']) + shelf = shelve.open(configs["simulation"]["shelveName"]) try: - port = shelf['port'] - print('shelf found') + port = shelf["port"] except KeyError: - port = configs['simulation']['defaultPort'] + port = configs["simulation"]["defaultPort"] itteration += 1 - print('not found') + print("not found") while True: try: con_str = "tcp:127.0.0.1:{}".format(port) print("Attempting to connect to {}".format(con_str)) - veh = connect(con_str, wait_ready=True, vehicle_class=v_type, \ - heartbeat_timeout=5, timeout=5) - shelf['port'] = port + veh = connect( + con_str, + wait_ready=True, + vehicle_class=v_type, + ) + veh.setup() + shelf["port"] = port shelf.close() break except (OSError, APIException): - port = configs['simulation']['defaultPort'] + itteration + port = configs["simulation"]["defaultPort"] + itteration itteration += 1 if itteration == 8: - print('make sure your simulator is running') + print("make sure your simulator is running") sys.exit(-1) return veh def to_quaternion(roll=0.0, pitch=0.0, yaw=0.0): - ''' + """ Convert degrees to quaternions - ''' + """ t0_val = cos(radians(yaw * 0.5)) t1_val = sin(radians(yaw * 0.5)) t2_val = cos(radians(roll * 0.5)) @@ -74,6 +66,7 @@ def to_quaternion(roll=0.0, pitch=0.0, yaw=0.0): return [w_val, x_val, y_val, z_val] + def parse_configs(argv): """Parses the .json file given as the first command line argument. If no .json file is specified, defaults to "configs.json". @@ -88,46 +81,70 @@ def parse_configs(argv): def new_output_file(): - '''Creates a new directory for the current date if not created already - and creates an output file for all console output in the directory.''' + """Creates a new directory for the current date if not created already + and creates an output file for all console output in the directory.""" curr_time = str(datetime.datetime.today()).split() date = curr_time[0] time = curr_time[1] # makes logs folder if not existing already try: - if sys.platform == "darwin" or sys.platform == "linux" or sys.platform == "linux2": - subprocess.check_output(["ls", "logs/"], shell=False, stderr=subprocess.STDOUT) + if ( + sys.platform == "darwin" + or sys.platform == "linux" + or sys.platform == "linux2" + ): + subprocess.check_output( + ["ls", "logs/"], shell=False, stderr=subprocess.STDOUT + ) elif sys.platform == "win32": - subprocess.check_output(["cd", "logs/"], shell=True, stderr=subprocess.STDOUT) + subprocess.check_output( + ["cd", "logs/"], shell=True, stderr=subprocess.STDOUT + ) else: raise Exception("Operating system not recognized") - except subprocess.CalledProcessError: - if sys.platform == "darwin" or sys.platform == "linux" or sys.platform == "linux2": + except subprocess.CalledProcessError as os_error: + if ( + sys.platform == "darwin" + or sys.platform == "linux" + or sys.platform == "linux2" + ): subprocess.call(["mkdir", "logs/"], shell=False) elif sys.platform == "win32": # No ls on Windows cmd subprocess.call(["mkdir", "logs/"], shell=True) else: - raise Exception("Operating system not recognized") + raise Exception("Operating system not recognized") from os_error # makes folder for the current date in logs folder if not existing already try: - if sys.platform == "darwin" or sys.platform == "linux" or sys.platform == "linux2": - subprocess.check_output(["ls", "logs/" + date], shell=False, stderr=subprocess.STDOUT) + if ( + sys.platform == "darwin" + or sys.platform == "linux" + or sys.platform == "linux2" + ): + subprocess.check_output( + ["ls", "logs/" + date], shell=False, stderr=subprocess.STDOUT + ) elif sys.platform == "win32": # No ls on Windows cmd - subprocess.check_output(["cd", "logs/" + date], shell=True, stderr=subprocess.STDOUT) + subprocess.check_output( + ["cd", "logs/" + date], shell=True, stderr=subprocess.STDOUT + ) else: raise Exception("Operating system not recognized") - except subprocess.CalledProcessError: - if sys.platform == "darwin" or sys.platform == "linux" or sys.platform == "linux2": + except subprocess.CalledProcessError as os_error: + if ( + sys.platform == "darwin" + or sys.platform == "linux" + or sys.platform == "linux2" + ): subprocess.call(["mkdir", "logs/" + date], shell=False) elif sys.platform == "win32": # Windows cmd handles mkdir path in a strange way subprocess.call(["mkdir", "logs\\" + date], shell=True) else: - raise Exception("Operating system not recognized") + raise Exception("Operating system not recognized") from os_error # open file for current time return open("logs/" + date + "/" + time.replace(":", ".") + ".txt", "w") @@ -143,5 +160,4 @@ def get_distance_metres(loc_a, loc_b): dlat = loc_b.lat - loc_a.lat dlong = loc_b.lon - loc_a.lon dalt = loc_b.alt - loc_a.alt - return sqrt((((dlat**2) + (dlong**2)) * 1.113195e5 ** 2) + (dalt ** 2)) - \ No newline at end of file + return sqrt((((dlat ** 2) + (dlong ** 2)) * 1.113195e5 ** 2) + (dalt ** 2)) diff --git a/src/vtol.py b/src/vtol.py deleted file mode 100644 index 49a7b8a..0000000 --- a/src/vtol.py +++ /dev/null @@ -1,257 +0,0 @@ -'''Automous tools for VTOL''' -import time -import json -from math import radians -from dronekit import VehicleMode, Vehicle, LocationGlobalRelative -from pymavlink import mavutil -from coms import Coms -from util import to_quaternion, get_distance_metres - - -class VTOL(Vehicle): - ''' VTOL basic state isolated''' - def __init__(self, *args): #pylint: disable=useless-super-delegation - super(VTOL, self).__init__(*args) - - def setup(self): - '''vtol specific steps needed before flight''' - - - # State, updated by XBee callback function - configs = None - start_mission = False # takeoff - pause_mission = False # vehicle will hover - stop_mission = False # return to start and land - - # Global status, updated by various functions - status = "ready" - MISSION_COMPLETED = False - coms = None - land_mode = 'LAND' - - # pylint: disable=no-self-use - def coms_callback(self, message): - '''callback for radio messages''' - parsed_message = json.loads(message.data) - #tuple of commands that can be executed - valid_commands = ("takeoff", "RTL") - #gives us the specific command we want the drone to executre - command = parsed_message['type'] - - print('Recieved message type:', type(parsed_message['type'])) - - #checking for valid command - if command not in valid_commands: - raise Exception("Error: Unsupported status for vehicle") - - #executes takeoff command to drone - if command == 'takeoff': - self.takeoff() - #executes land command to drone - elif command == 'land': - self.land() - - # TODO respond to xbee messagge - data = json.loads(message.data) - print(data['type']) - - def setup_coms(self): - '''sets up communication radios''' - # TODO set up coms and callback - print('Initializing Coms') - self.coms = Coms(self.configs, self.coms_callback) - - - def start_auto_mission(self): - '''Arms and starts an AUTO mission loaded onto the vehicle''' - while not self.is_armable: - print(" Waiting for vehicle to initialise...") - time.sleep(1) - - self.mode = VehicleMode("GUIDED") - self.armed = True - - while not self.armed: - print(" Waiting for arming...") - time.sleep(1) - - self.commands.next = 0 - self.mode = VehicleMode("AUTO") - - msg = self.message_factory.command_long_encode( - 0, 0, # target_system, target_component - mavutil.mavlink.MAV_CMD_MISSION_START, #command - 0, #confirmation - 0, 0, 0, 0, 0, 0, 0) # param 1 ~ 7 not used - # send command to vehicle - self.send_mavlink(msg) - - self.commands.next = 0 - - - def takeoff(self): - '''Commands drone to take off by arming vehicle and flying to altitude''' - print("Pre-arm checks") - while not self.is_armable: - print("Waiting for vehicle to initialize") - time.sleep(1) - - print("Arming motors") - # Vehicle should arm in GUIDED mode - self.mode = VehicleMode("GUIDED") - self.armed = True - - while not self.armed: - print("Waiting to arm vehicle") - time.sleep(1) - - print("Taking off") - - altitude = self.configs['altitude'] - self.simple_takeoff(altitude) # take off to altitude - - # Wait until vehicle reaches minimum altitude - while self.location.global_relative_frame.alt < altitude * 0.95: - print("Altitude: " + str(self.location.global_relative_frame.alt)) - time.sleep(1) - - print("Reached target altitude") - - def go_to(self, point): - '''Commands drone to fly to a specified point perform a simple_goto ''' - - self.simple_goto(point, self.configs["air_speed"]) - - while True: - distance = get_distance_metres(self.location.global_relative_frame, point) - if distance > self.configs['waypoint_tolerance']: - print("Distance remaining:", distance) - time.sleep(1) - else: - break - print("Target reached") - - - def land(self): - '''Commands vehicle to land''' - self.mode = VehicleMode(self.land_mode) - - print("Landing...") - - while self.location.global_relative_frame.alt > 0: - print("Altitude: " + str(self.location.global_relative_frame.alt)) - time.sleep(1) - - print("Landed") - - print("Sleeping...") - time.sleep(5) - - def set_altitude(self, alt): - '''Sets altitude of quadcopter using an "alt" parameter''' - print("Setting altitude:") - destination = LocationGlobalRelative(self.location.global_relative_frame.lat, \ - self.location.global_relative_frame.lon, alt) - self.go_to(destination) - print("Altitude reached") - - def change_status(self, new_status): - ''':param new_status: new vehicle status to change to (refer to GCS formatting)''' - if new_status not in ("ready", "running", "waiting", "paused", "error"): - raise Exception("Error: Unsupported status for vehicle") - self.status = new_status - - - def send_attitude_target( - self, - roll_angle=0.0, - pitch_angle=0.0, - yaw_angle=None, - yaw_rate=0.0, - use_yaw_rate=False, - thrust=0.5, - ): - ''' - use_yaw_rate: the yaw can be controlled using yaw_angle OR yaw_rate. - When one is used, the other is ignored by Ardupilot. - thrust: 0 <= thrust <= 1, as a fraction of maximum vertical thrust. - Note that as of Copter 3.5, thrust = 0.5 triggers a special case in - the code for maintaining current altitude. - ''' - if yaw_angle is None: - # this value may be unused by the vehicle, depending on use_yaw_rate - yaw_angle = self.attitude.yaw - # Thrust > 0.5: Ascend - # Thrust == 0.5: Hold the altitude - # Thrust < 0.5: Descend - msg = self.message_factory.set_attitude_target_encode( - 0, # time_boot_ms - 1, # Target system - 1, # Target component - 0b00000000 if use_yaw_rate else 0b00000100, - to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion - 0, # Body roll rate in radian - 0, # Body pitch rate in radian - radians(yaw_rate), # Body yaw rate in radian/second - thrust, # Thrust - ) - self.send_mavlink(msg) - - - def set_attitude( - self, - roll_angle=0.0, - pitch_angle=0.0, - yaw_angle=None, - yaw_rate=0.0, - use_yaw_rate=False, - thrust=0.5, - duration=0, - ): - ''' - Note that from AC3.3 the message should be re-sent more often than every - second, as an ATTITUDE_TARGET order has a timeout of 1s. - In AC3.2.1 and earlier the specified attitude persists until it is canceled. - The code below should work on either version. - Sending the message multiple times is the recommended way. - ''' - self.send_attitude_target( - roll_angle, pitch_angle, yaw_angle, yaw_rate, use_yaw_rate, thrust - ) - start = time.time() - while time.time() - start < duration: - self.send_attitude_target( - roll_angle, pitch_angle, yaw_angle, yaw_rate, use_yaw_rate, thrust - ) - time.sleep(0.1) - # Reset attitude, or it will persist for 1s more due to the timeout - self.send_attitude_target(0, 0, 0, 0, True, thrust) - - - def update_thread(self, address): - ''':param vehicle: vehicle object that represents drone - :param vehicle_type: vehicle type from configs file''' - print("Starting update thread\n") - - while not self.MISSION_COMPLETED: - location = self.location.global_frame - # Comply with format of 0 - 1 and check that battery level is not null - battery_level = self.battery.level / 100.0 if self.battery.level else 0.0 - update_message = { - "type": "update", - "time": round(time.clock() - self.coms.con_timestamp) + self.coms.gcs_timestamp, - "sid": self.configs["vehicle_id"], - "tid": 0, # the ID of the GCS is 0 - "id": self.coms.new_msg_id(), - - "vehicleType": "VTOL", - "lat": location.lat, - "lon": location.lon, - "status": self.status, - # TODO heading - "battery": battery_level - } - - self.coms.send_till_ack(address, update_message, update_message['id']) - time.sleep(1) - self.change_status("ready") diff --git a/tox.ini b/tox.ini index 8f86631..58b8b5c 100644 --- a/tox.ini +++ b/tox.ini @@ -1,6 +1,6 @@ [tox] minversion = 2.0 -envlist = lint, vtol +envlist = lint, hex skipsdist = True [testenv] @@ -11,8 +11,8 @@ passenv = * [testenv:lint] basepython=python3.7 -commands = pylint --rcfile=.pylintrc examples src +commands=pylint --rcfile=.pylintrc src -[testenv:vtol] +[testenv:hex] basepython=python3.7 commands = python3 -m pytest \ No newline at end of file