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/simulator/sim.py b/simulator/sim.py index 9a6d419..7d5dcae 100644 --- a/simulator/sim.py +++ b/simulator/sim.py @@ -16,7 +16,7 @@ if DO_DOWNLOAD: SIM.download('copter', '3.3', verbose=False) -SIM.launch(['--model', 'quad', ], await_ready=True, restart=False) +SIM.launch(['--model', 'hex', ], await_ready=True, restart=False) print("Simulator lanched") while True: diff --git a/src/quad.py b/src/hex.py similarity index 96% rename from src/quad.py rename to src/hex.py index d43d4a4..a5c6a6b 100644 --- a/src/quad.py +++ b/src/hex.py @@ -1,4 +1,4 @@ -"""Automous tools for VTOL""" +"""Automous tools for HEX""" import time import json from dronekit import VehicleMode, Vehicle, LocationGlobalRelative @@ -6,8 +6,8 @@ from util import get_distance_metres -class QUAD(Vehicle): - """ VTOL basic state isolated""" +class HEX(Vehicle): + """ HEX basic state isolated""" coms = None diff --git a/src/main.py b/src/main.py index 9e538f6..fb813e6 100644 --- a/src/main.py +++ b/src/main.py @@ -1,13 +1,15 @@ -"""main executable for setting up VTOL""" +"""main executable for setting up HEX""" import json from util import setup_vehicle -from quad import QUAD +from hex import HEX def main(configs): - """Configure vtol and ready for mission""" - quad = setup_vehicle(configs, QUAD) - quad.land() + """Configure hexcopter and ready for mission""" + hexa = setup_vehicle(configs, HEX) + hexa.takeoff() + hexa.go_to(27.28, 153.01) + hexa.land() if __name__ == "__main__": diff --git a/src/tests/test_quad.py b/src/tests/test_hex.py similarity index 82% rename from src/tests/test_quad.py rename to src/tests/test_hex.py index 4be3eb9..a8e0524 100644 --- a/src/tests/test_quad.py +++ b/src/tests/test_hex.py @@ -1,7 +1,7 @@ -"""test for vtol.py""" +"""test for hex.py""" from dronekit_sitl import start_default from util import setup_vehicle -from quad import QUAD +from hex import HEX CONFIGS = { "vehicle_simulated": True, @@ -16,11 +16,11 @@ CON_STR = start_default().connection_string() with open("configs.json", "r") as data: - VEHICLE = setup_vehicle(CONFIGS, QUAD) + VEHICLE = setup_vehicle(CONFIGS, HEX) def test_takeoff(): - """quadcopter dronekit-sitl takeoff""" + """hexcopter dronekit-sitl takeoff""" global VEHICLE # pylint: disable=global-statement VEHICLE.takeoff() alt = VEHICLE.location.global_relative_frame.alt @@ -28,7 +28,7 @@ def test_takeoff(): def test_land(): - """quadcopter dronekit-sitl land""" + """hexcopter dronekit-sitl land""" global VEHICLE # pylint: disable=global-statement VEHICLE.land() alt = VEHICLE.location.global_relative_frame.alt diff --git a/src/util.py b/src/util.py index 02ab9cd..86c7f1b 100644 --- a/src/util.py +++ b/src/util.py @@ -1,4 +1,4 @@ -"""Util methods for VTOL""" +"""Util methods for HEX""" import json import datetime import subprocess @@ -10,7 +10,7 @@ def setup_vehicle(configs, v_type): - """Sets up self as a quadplane vehicle""" + """Sets up self as a hexcopter vehicle""" v_type = partial(v_type, configs) veh = scan_ports(configs, v_type) return veh diff --git a/tox.ini b/tox.ini index 00980e0..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] @@ -13,6 +13,6 @@ passenv = * basepython=python3.7 commands=pylint --rcfile=.pylintrc src -[testenv:vtol] +[testenv:hex] basepython=python3.7 commands = python3 -m pytest \ No newline at end of file