Feature/support new qgroundcontrol

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

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

anaconda_avionics/utilities/xbee.py

@@ -12,14 +12,15 @@ 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']
 
         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()