diff --git a/tests/__init__.py b/anaconda_avionics/tests/__init__.py similarity index 100% rename from tests/__init__.py rename to anaconda_avionics/tests/__init__.py diff --git a/anaconda_avionics/tests/test_mission_plan_parser.py b/anaconda_avionics/tests/test_mission_plan_parser.py new file mode 100644 index 0000000..161952d --- /dev/null +++ b/anaconda_avionics/tests/test_mission_plan_parser.py @@ -0,0 +1,56 @@ +""" +This test will confirm the functionality of the system starting from QGS upload +to drone download and parsing. The flow of the test will be as follows: + 1) Load the unit test data station in Mission Mule's QGroundControl. + 2) Load the unit test landing sequance in Mission Mule's QGroundControl. + 3) Auto-Generate the mission and upload to the drone. + 4) From the anaconda_avionics directory, run + python -m test_mission_plan_parser +""" + +from utilities.mission_plan_parser import MissionPlanParser +from utilities.data_station import DataStation +from utilities.landing_waypoint import LandingWaypoint +from dronekit import connect + +connection_string = "/dev/ttyS0" + + + +if __name__ == '__main__': + + # create the expected values + expected_data_stations = [] + expected_data_stations.append(DataStation(24.521592, 54.435452, 60, '03')) + expected_data_stations.append(DataStation(24.522882, 54.437273, 60, '07')) + expected_data_stations.append(DataStation(24.526051, 54.432802, 60, '17')) + expected_landing_waypoints = [] + expected_landing_waypoints.append(LandingWaypoint(24.527945, 54.430411, 50)) + expected_landing_waypoints.append(LandingWaypoint(24.527945, 54.430411, 50)) + expected_landing_waypoints.append(LandingWaypoint(24.523096, 54.433605, 50)) + + + # connect to vehicle + vehicle = connect(connection_string, baud=57600, wait_ready=True) + + # instatiate MissionPlanParser object + mission_plan_parser = MissionPlanParser(vehicle) + + # extract waypoints + data_stations, landing_waypoints = mission_plan_parser.extract_waypoints() + + data_stations_test_passed = True + for i in range(3): + data_stations_test_passed &= expected_data_stations[i].lat = data_stations[i].lat + data_stations_test_passed &= expected_data_stations[i].lon = data_stations[i].lon + data_stations_test_passed &= expected_data_stations[i].alt = data_stations[i].alt + data_stations_test_passed &= expected_data_stations[i].id = data_stations[i].id + + landing_sequence_test_passed = True + for i in range(3): + landing_sequence_test_passed &= expected_landing_waypoints[i].lat = landing_waypoints[i].lat + landing_sequence_test_passed &= expected_landing_waypoints[i].lon = landing_waypoints[i].lon + landing_sequence_test_passed &= expected_landing_waypoints[i].alt = landing_waypoints[i].alt + + print "Data Station Test Passed: ", data_stations_test_passed + print "Landing Sequence Test Passed: ", landing_sequence_test_passed diff --git a/tests/test_mission_preparation.py b/anaconda_avionics/tests/test_mission_preparation.py similarity index 100% rename from tests/test_mission_preparation.py rename to anaconda_avionics/tests/test_mission_preparation.py diff --git a/tests/test_navigation.py b/anaconda_avionics/tests/test_navigation.py similarity index 100% rename from tests/test_navigation.py rename to anaconda_avionics/tests/test_navigation.py diff --git a/tests/test_sftp_client.py b/anaconda_avionics/tests/test_sftp_client.py similarity index 100% rename from tests/test_sftp_client.py rename to anaconda_avionics/tests/test_sftp_client.py diff --git a/tests/test_xbee.py b/anaconda_avionics/tests/test_xbee.py similarity index 100% rename from tests/test_xbee.py rename to anaconda_avionics/tests/test_xbee.py diff --git a/tests/unit_tests.py b/anaconda_avionics/tests/unit_tests.py similarity index 100% rename from tests/unit_tests.py rename to anaconda_avionics/tests/unit_tests.py diff --git a/anaconda_avionics/utilities/mission_plan_parser.py b/anaconda_avionics/utilities/mission_plan_parser.py index 2dc5545..141b72d 100644 --- a/anaconda_avionics/utilities/mission_plan_parser.py +++ b/anaconda_avionics/utilities/mission_plan_parser.py @@ -1,54 +1,101 @@ """ -This module takes in a CSV file in the command line and returns a list of Camera objects -and LandingWaypoint objects that are later used to upload and monitor data mule missions. -Eventually, this will accept MavLink commands instead-of/in-conjuction-with the CSV file. +This module takes in a vehicle object, reads in the mission and returns a list +of Camera objects and LandingWaypoint objects that are later used to upload and +monitor data mule missions. """ -from anaconda_avionics.utilities import DataStation -from anaconda_avionics.utilities import LandingWaypoint +#from anaconda_avionics.utilities. +from data_station import DataStation +#from anaconda_avionics.utilities. +from landing_waypoint import LandingWaypoint - -# TODO: reconfigure this to accept QGroundControl mission plan and execute accordingly class MissionPlanParser(object): - __data_waypoints = None # Waypoints to be loitered over - __landing_waypoints = None # Waypoints to be flown through in landing sequence + def __init__(self, _autopilot): + self._autopilot = _autopilot + self._data_stations = [] + self._landing_waypoints = [] - def __init__(self, _data_waypoints, _landing_waypoints): - self.__data_waypoints = _data_waypoints - self.__landing_waypoints = _landing_waypoints + # load the commands + self._cmds = self._autopilot.commands + self._cmds.download() + self._cmds.wait_ready() + self._number_of_commands = self._cmds.count def extract_waypoints(self): """ - Reads CSV file and returns list of LandingWaypoint objects and list of Camera objects. + Downloads the MAVlink commadns from the pixhawk and reurns a list of + Camera and LandingWaypoint objects """ - # Raw latitude, longitude, and altitude for CAMERA TRAPS translated to - # Camera objects - number_of_data_stations = len(self.__data_waypoints) - data_stations = [] - for line in range(number_of_data_stations): - if not self.__data_waypoints[line][0].isalpha(): # Not data column descriptor - new_data_station = DataStation( # TODO: change Camera object to "DataStation" - float(self.__data_waypoints[line][0]), - float(self.__data_waypoints[line][1]), - int(self.__data_waypoints[line][2]), - str(self.__data_waypoints[line][3])) - data_stations.append(new_data_station) - - # Raw latitude, longitude, and altitude for LANDING SEQUENCE translated to - # LandingWaypoints - landing_waypoints = [] - waypoint_num = 0 - number_of_waypoints = len(self.__landing_waypoints) - for line in range(number_of_waypoints): - if not self.__landing_waypoints[line][0].isalpha(): # Not data column descriptor - waypoint_num += 1 - new_landing_waypoint = LandingWaypoint( - waypoint_num, - float(self.__landing_waypoints[line][0]), - float(self.__landing_waypoints[line][1]), - float(self.__landing_waypoints[line][2])) - landing_waypoints.append(new_landing_waypoint) - - return data_stations, landing_waypoints + # Read in list of MAVlink commands from the pixhakw. + cmd_index = 1 + # command 0 is always Mission Start + # command 1 is always Takeoff + while (cmd_index < self._number_of_commands): + # TODO: distinguish between landing, takeoff and obstacle avoidance + + # get the command ID + + + cmd = self._autopilot._wploader.wp(cmd_index) + print cmd.command + # determine if the command indicates a data station + if (cmd.command == 16): # 16 corresponds to MAV_CMD_NAV_WAYPOINT + new_waypoint = LandingWaypoint(cmd_index, cmd.x, cmd.y, cmd.z) + self._landing_waypoints.append(new_waypoint) + + elif (cmd.command == 189): # 189 corresponds to DO_LAND_START + new_waypoint = LandingWaypoint(cmd_index, cmd.x, cmd.y, cmd.z) + self._landing_waypoints.append(new_waypoint) + + elif (cmd.command == 12): # 21 corresponds to MAV_CMD_NAV_LAND + new_waypoint = LandingWaypoint(cmd_index, cmd.x, cmd.y, cmd.z) + self._landing_waypoints.append(new_waypoint) + + elif (cmd.command == 31): # 31 corresponds to MAV_CMD_NAV_LOITER_TO_ALT + new_waypoint = LandingWaypoint(cmd_index, cmd.x, cmd.y, cmd.z) + self._landing_waypoints.append(new_waypoint) + + elif (cmd.command == 201): # corresponds to a data station! MAV_CMD_DO_SET_ROI + new_data_station = DataStation(cmd.x, cmd.y, cmd.z, cmd.param4) + self._data_stations.append(new_data_station) + # the next waypoint is MAV_CMD_NAV_WAYPOINT + # at the same location + # TODO: add check to make sure the above is true + cmd_index += 1 + + else: + # we should throw some king of global error for cmds that + # the drone is not expecting + pass + + cmd_index += 1 + + # Return the results + return self._data_stations, self._landing_waypoints + +if __name__ == '__main__': + from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command + + connection_string = raw_input("Enter connection string: ") + print 'Connecting to vehicle on: %s' % connection_string + vehicle = connect(connection_string, baud=57600, wait_ready=True) + print 'Connection succesful!' + + parser = MissionPlanParser(vehicle) + parser.extract_waypoints() + + print "Landing Waypoints: " + for waypoint in parser._landing_waypoints: + print "Lat: ", waypoint.lat, "Lon: ", waypoint.lon, "Alt: ", waypoint.alt + + print "\n----------------------\n" + + for ds in parser._data_stations: + print "Data Station: ", ds.identity + print " Lat: ", ds.lat, "Lon: ", ds.lon, "Alt: ", ds.alt + print " Timeout Event: ", ds.timeout + print " Drone Arrived: ", ds.drone_arrived + print " Download Start: ", ds.download_started + print " Download Complete: ", ds.download_complete diff --git a/anaconda_avionics/utilities/xbee.py b/anaconda_avionics/utilities/xbee.py index 1e5011f..7893236 100644 --- a/anaconda_avionics/utilities/xbee.py +++ b/anaconda_avionics/utilities/xbee.py @@ -12,6 +12,7 @@ def __init__(self, serial_port="/dev/ttyUSB0"): self.encode = None self.decode = None self.data_station_idens = None + self.timeout = 360 # seconds self.preamble_out = ['s', 't', 'r', 'e', 'e', 't'] self.preamble_in = ['c', 'a', 't'] @@ -19,7 +20,7 @@ def __init__(self, serial_port="/dev/ttyUSB0"): while True: try: if not "DEVELOPMENT" in os.environ: # Don't connect to XBee while in development - self.xbee_port = serial.Serial(serial_port, 9600, timeout=5) + self.xbee_port = serial.Serial(serial_port, 57600, timeout=5) logging.info("Connected to XBee") else: logging.info("In development mode, not connecting to XBee") @@ -30,14 +31,18 @@ def __init__(self, serial_port="/dev/ttyUSB0"): # TODO: make single dictionary self.encode = { - 'POWER_ON' : '1', - 'POWER_OFF' : '2', - 'EXTEND_TIME' : '3' + 'POWER_ON' : '1', + 'POWER_OFF' : '2', + 'EXTEND_TIME' : '3', + 'RESET_ID' : '4', + 'REQUEST_GPS' : '5', } self.decode = { '1' : 'POWER_ON', '2' : 'POWER_OFF', - '3' : 'EXTEND_TIME' + '3' : 'EXTEND_TIME', + '4' : 'RESET_ID', + '5' : 'REQUEST_GPS', } self.data_station_idens = self.read_iden_map() @@ -63,6 +68,38 @@ def send_command(self, identity, command): logging.debug("xBee port write: %s" % self.encode[command]) self.xbee_port.write(self.encode[command]) + def change_id(self, identity, new_id): + self.send_command(identity, 'RESET_ID') + self.xbee_port.write('<'+new_id+'>') + + def get_gps_data(self, identity): + self.send_command(identity, 'REQUEST_GPS') + time_start = time.time() + incoming_byte = '' + prelimitor_received = False + postlmitor_received = False + while True: + if (self.xbee_port.in_waiting > 0): + if(self.xbee_port.read() == '<'): + break + if ((time.time() - time_start) > self.timeout): + return '-1' + time.sleep(0.1) + + gps_coordinates = '' + while True: + if (self.xbee_port.in_waiting > 0): + incoming_byte = self.xbee_port.read() + + if (incoming_byte == '>'): + return gps_coordinates + else: + gps_coordinates += incoming_byte + + if ((time.time() - time_start) > self.timeout): + return '-1' + time.sleep(0.1) + def acknowledge(self, identity, command): """ Called after command is sent @@ -108,3 +145,22 @@ def acknowledge(self, identity, command): identity_index = 0 return False # Unsuccessful ACK + +if __name__ == "__main__": + serial_port = raw_input("Enter serial port: ") + xbee = XBee(serial_port) + while True: + try: + command = raw_input("Enter Command \nPOWER_ON: 1\nPOWER_OFF: 2\nEXTEND_TIME: 3\nRESET_ID: 4\nREQUEST_GPS: 5\nCommand: ") + try: + if (command == '4'): + new_id = raw_input("New ID: ") + xbee.change_id('street_cat', new_id) + elif (command == '5'): + print xbee.get_gps_data('street_cat') + else: + xbee.send_command('street_cat', xbee.decode[command]) + except KeyError: + pass + except KeyboardInterrupt: + xbee.xbee_port.close() diff --git a/tests/test_mission_plan_parser.py b/tests/test_mission_plan_parser.py deleted file mode 100644 index e69de29..0000000