Added Status functionality, and created an additional demo script to …

…show that.

README.md

@@ -40,7 +40,7 @@ from fly_tello import FlyTello
 with FlyTello(['XXX']) as fly:
     fly.set_ap_wifi(ssid='MY_SSID', password='MY_PASSWORD')
 ```
-The above code initialises FlyTello, and then sets the SSID and Password you supply.  You can get the Serial Number for your Tello from a tiny sticker inside the battery compartment, but by using `'XXX'` here FlyTello will print the Serial Number to the Terminal anyway.  You should usually provide the Serial Number when initialising FlyTello, but it's not essential here because there's only one.
+The above code initialises FlyTello, and then sets the SSID and Password you supply.  You can get the Serial Number for your Tello from a tiny sticker inside the battery compartment, but by using `'XXX'` here FlyTello will print the Serial Number to the Console anyway.  You should usually provide the Serial Number when initialising FlyTello, but it's not essential here because there's only one.
 
 **Project Structure**
 
@@ -106,5 +106,5 @@ There are some limitations of what can be done with this project and the Tello E
 The project as it is currently is enough to fly one or more Tello Edu drones via a simple yet sophisticated set of controls.  Expanding its capabilities is easy, with layers of modules which expose increasingly more detailed / low-level functionality.  I'd suggest adding or changing:
 * Position Tracking.  By tracking the relative position of each Tello from when it launches, this will enable behaviours such as "return to start", and will e.g. allow Mission Pad locations to be shared with other Tellos in the swarm - a pre-requisite for collaborative swarm behaviour.  Clearly accuracy will decrease over time, but could be regularly restored using the `reorient()` method described above.
 * Better Error Checking.  Some error checking is already implemented, but it's incomplete.  Getting the arc radius correct for a curve is sometimes difficult, and this project could be more helpful in identifying the errors and suggesting valid alternative values.
-* Command Stream & Logging.  Currently all commands either sent or received are printed to the Python Terminal.  These would be better saved in a detailed log file, so that only key information is presented to the user in the Terminal.
+* Command Stream & Logging.  Currently all commands either sent or received are printed to the Python Console.  These would be better saved in a detailed log file, so that only key information is presented to the user in the Console.
 

comms_manager.py

@@ -39,7 +39,7 @@ def __init__(self):
         # Reference to all active Tellos
         self.tellos = []
 
-    def init_tellos(self, sn_list, first_ip=1, last_ip=254):
+    def init_tellos(self, sn_list, get_status=False, first_ip=1, last_ip=254):
         """ Search the network until found the specified number of Tellos, then get each Tello ready for use.
 
             This must be run once; generally the first thing after initiating CommsManager.
@@ -49,6 +49,7 @@ def init_tellos(self, sn_list, first_ip=1, last_ip=254):
             Finally, each Tello is queried for its serial number, which is stored in the Tello object with its number.
 
             :param sn_list: List of serial numbers, in order we want to number the Tellos.
+            :param get_status: True to listen for and record the status messages from the Tellos.
             :param first_ip: If known, we can specify a smaller range of IP addresses to speed up the search.
             :param last_ip: If known, we can specify a smaller range of IP addresses to speed up the search.
         """
@@ -95,6 +96,13 @@ def init_tellos(self, sn_list, first_ip=1, last_ip=254):
             command_handler_thread.daemon = True
             command_handler_thread.start()
 
+        # Start the status_handler, if needed.  This receives and constantly updates the status of each Tello.
+        if get_status:
+            self.status_socket.bind(('', self.status_port))
+            self.status_thread = threading.Thread(target=self._status_thread)
+            self.status_thread.daemon = True
+            self.status_thread.start()
+
         # Query each Tello to get its serial number - saving the cmd_id so we can match-up responses when they arrive
         tello_cmd_id = []
         for tello in self.tellos:
@@ -324,3 +332,29 @@ def _receive_thread(self):
                 if not self.terminate_comms:
                     # Report socket errors, but only if we've not told it to terminate_comms.
                     print('[Socket Error]Exception socket.error : %s' % exc)
+
+    def _status_thread(self):
+        """ Listen continually to status from the Tellos - should run in its own thread.
+
+            Listens for status messages from each Tello, and saves them in the Tello object as they arrive.
+        """
+
+        while not self.terminate_comms:
+            try:
+                response, ip = self.status_socket.recvfrom(1024)
+                response = response.decode()
+                if response == 'ok':
+                    continue
+                ip = ''.join(str(ip[0]))
+                tello = self._get_tello(ip)
+                tello.status.clear()
+                status_parts = response.split(';')
+                for status_part in status_parts:
+                    key_value = status_part.split(':')
+                    if len(key_value) == 2:
+                        tello.status[key_value[0]] = key_value[1]
+
+            except socket.error as exc:
+                if not self.terminate_comms:
+                    # Report socket errors, but only if we've not told it to terminate_comms.
+                    print('[Socket Error]Exception socket.error : %s' % exc)

demo_all_functions.py

@@ -20,22 +20,22 @@
 # Control the flight
 with FlyTello(my_tellos) as fly:
     fly.takeoff()
-    fly.forward(50)
-    fly.back(50)
-    fly.reorient_on_pad(100, 'm-2')
+    fly.forward(dist=50)
+    fly.back(dist=50)
+    fly.reorient(height=100, pad='m-2')
     with fly.sync_these():
-        fly.left(50, 1)
-        fly.right(50, 2)
+        fly.left(dist=50, tello=1)
+        fly.right(dist=50, tello=2)
     with fly.sync_these():
-        fly.flip('right', 1)
-        fly.flip('left', 2)
-    fly.reorient_on_pad(100, 'm-2')
-    fly.straight(75, 75, 0, 100)
-    fly.curve(-55, -20, 0, -75, -75, 0, 60)
+        fly.flip(direction='right', tello=1)
+        fly.flip(direction='left', tello=2)
+    fly.reorient(height=100, pad='m-2')
+    fly.straight(x=75, y=75, z=0, speed=100)
+    fly.curve(x1=-55, y1=-20, z1=0, x2=-75, y2=-75, z2=0, speed=60)
     with fly.sync_these():
-        fly.rotate_cw(360, 1)
-        fly.rotate_ccw(360, 2)
-    fly.straight_from_pad(50, 0, 75, 100, 'm-2')
-    fly.flip('back')
-    fly.reorient_on_pad(50, 'm-2')
+        fly.rotate_cw(angle=360, tello=1)
+        fly.rotate_ccw(angle=360, tello=2)
+    fly.straight_from_pad(x=50, y=0, z=75, speed=100, pad='m-2')
+    fly.flip(direction='back')
+    fly.reorient(height=50, pad='m-2')
     fly.land()

demo_status.py

@@ -0,0 +1,26 @@
+from fly_tello import FlyTello
+my_tellos = list()
+
+
+#
+# SIMPLE EXAMPLE - MOST BASIC FLIGHT TO SHOW STATUS MESSAGES
+#
+# SETUP: Any number of Tellos
+#
+
+
+#
+# MAIN FLIGHT CONTROL LOGIC
+#
+
+# Define the Tello's we're using, in the order we want them numbered
+my_tellos.append('0TQDFC6EDBBX03')
+my_tellos.append('0TQDFC6EDB4398')
+
+# Control the flight
+with FlyTello(my_tellos, get_status=True) as fly:
+    fly.print_status(sync=True)
+    fly.takeoff()
+    fly.print_status(sync=True)
+    fly.land()
+    fly.print_status(sync=True)

fly_tello.py

@@ -1,6 +1,6 @@
 import time
 import threading
-from typing import Union
+from typing import Union, Optional
 from contextlib import contextmanager
 from comms_manager import CommsManager
 
@@ -19,15 +19,15 @@ class FlyTello:
     # CLASS INITIALISATION AND CONTEXT HANDLER
     #
 
-    def __init__(self, tello_sn_list: list, first_ip: int=1, last_ip: int=254):
+    def __init__(self, tello_sn_list: list, get_status=False, first_ip: int=1, last_ip: int=254):
         """ Initiate FlyTello, starting up CommsManager, finding and initialising our Tellos, and reporting battery.
 
             :param tello_sn_list: List of serial numbers, in the order we want to number the Tellos.
             :param first_ip: Optionally, we can specify a smaller range of IP addresses to speed up the search.
             :param last_ip: Optionally, we can specify a smaller range of IP addresses to speed up the search.
         """
         self.tello_mgr = CommsManager()
-        self.tello_mgr.init_tellos(sn_list=tello_sn_list, first_ip=first_ip, last_ip=last_ip)
+        self.tello_mgr.init_tellos(sn_list=tello_sn_list, get_status=get_status, first_ip=first_ip, last_ip=last_ip)
         self.tello_mgr.queue_command('battery?', 'Read', 'All')
         self.individual_behaviour_threads = []
         self.in_sync_these = False
@@ -504,12 +504,37 @@ def flight_complete(self, tello: int) -> None:
         """
         self.tello_mgr.get_tello(tello).flight_complete = True
 
+    #
+    # STATUS MESSAGE PROCESSING
+    #
+
+    def print_status(self, tello: Union[int, str]='All', sync: bool=False) -> None:
+        """ Print the entire Status Message to the Python Console, for the specified Tello(s). """
+        if sync and not self.in_sync_these:
+            self.tello_mgr.wait_sync()
+        if tello == 'All':
+            for tello in self.tello_mgr.tellos:
+                print('Tello %d Status: %s' % (tello.num, tello.status))
+        else:
+            tello = self.tello_mgr.get_tello(num=tello)
+            print('Tello %d Status: %s' % (tello.num, tello.status))
+
+    def get_status(self, key: str, tello: int, sync: bool=False) -> Optional[str]:
+        """ Return the value of a specific key from an individual Tello  """
+        if sync and not self.in_sync_these:
+            self.tello_mgr.wait_sync()
+        tello = self.tello_mgr.get_tello(num=tello)
+        if key in tello.status:
+            return tello.status[key]
+        return None
+
     #
     # PRIVATE SHORTCUT METHODS
     #
 
     def _command(self, command, command_type, tello_num, sync):
         if sync and tello_num == 'All' and not self.in_sync_these:
+            # TODO: Review whether tello_num=='All' should preclude wait_sync - might want to keep it!
             self.tello_mgr.wait_sync()
         self.tello_mgr.queue_command(command, command_type, tello_num)
 

tello.py

@@ -17,6 +17,7 @@ def __init__(self, ip):
         self.command_queue = []
         self.log = []
         self.flight_complete = False
+        self.status = {}
 
     #
     # COMMAND_QUEUE AND LOG MANAGEMENT