Make example keep only points separated by at least 3m

Author: hamishwillee

docs/examples/flight_replay.rst

@@ -1,22 +1,27 @@
-=========================
+======================
 Example: Flight Replay
-=========================
+======================
 
-This example creates and runs a waypoint mission using position information in a TLOG file.
+This example creates and runs a waypoint mission using position information from a TLOG file.
 
-The log used in this example contains around 2700 points. We reduce this to a smaller number 
-(that is able to fit on the autopilot) by taking 100 points that are evenly spread across the range. 
-After 60 seconds the mission is artificially ended by setting the mode to RTL (return to launch).
+The log used in this example contains around 2700 points. This is too many points to upload
+to the autopilot (and to usefully display). Instead we only add points that are more than 
+3 metres away from the previously kept point, and only store 99 points in total. 
+After 60 seconds the mission is ended by setting the mode to RTL (return to launch).
 
 .. figure:: flight_replay_example.png
     :width: 50%
 
-    Mission generated from log
+    99 point mission generated from log
+
 
 .. note::
 
-    A more detailed example might determine the best points for the mission
-    by mapping the path to lines and spline curves.
+    The method used to reduce the number of points is fairly effective, but we
+    could do better by grouping some of the waypoints, and mapping others using
+    spline waypoints. This might be a 
+    `fun research project <https://github.com/dronekit/dronekit-python/issues/561>`_!
+
 
 
 Running the example
@@ -45,22 +50,23 @@ In summary, after cloning the repository:
    .. code:: bash
 
        Generating waypoints from tlog...
-        Generated 96 waypoints from tlog
+        Generated 100 waypoints from tlog
        Starting copter simulator (SITL)
        SITL already Downloaded.
-       Connecting to vehicle on: tcp:127.0.0.1:5760
+       Connecting to vehicle on: tcp:127.0.0.1:5
        >>> APM:Copter V3.3 (d6053245)
        >>> Frame: QUAD
        >>> Calibrating barometer
        >>> Initialising APM...
        >>> barometer calibration complete
        >>> GROUND START
-       Uploading 96 waypoints to vehicle...
+       Uploading 100 waypoints to vehicle...
        Arm and Takeoff
         Waiting for vehicle to initialise...
        >>> flight plan received
         Waiting for arming...
-        ...
+        Waiting for arming...
+        Waiting for arming...
         Waiting for arming...
        >>> ARMING MOTORS
        >>> GROUND START
@@ -69,53 +75,35 @@ In summary, after cloning the repository:
         Waiting for arming...
        >>> ARMING MOTORS
         Taking off!
+        Altitude: 0.000000 < 28.500000
         Altitude: 0.010000 < 28.500000
-        Altitude: 0.020000 < 28.500000
         ...
-        Altitude: 26.150000 < 28.500000
-        Altitude: 28.170000 < 28.500000
+        Altitude: 26.350000 < 28.500000
+        Altitude: 28.320000 < 28.500000
         Reached target altitude of ~30.000000
        Starting mission
-       Distance to waypoint (1): 6.31671220061
-       Distance to waypoint (1): 5.54023406731
+       Distance to waypoint (1): 3.02389745499
        >>> Reached Command #1
-       Distance to waypoint (2): 4.05805003875
-       ...
-       Distance to waypoint (2): 4.66600036548
+       Distance to waypoint (2): 5.57718471895
+       Distance to waypoint (2): 4.1504263025
        >>> Reached Command #2
-       Distance to waypoint (3): 1.49371523482
-       Distance to waypoint (3): 0.13542601646
-       Distance to waypoint (3): 0.708432959397
+       Distance to waypoint (3): 0.872847106279
+       Distance to waypoint (3): 1.88967925144
+       Distance to waypoint (3): 2.16157704522
        >>> Reached Command #3
-       Distance to waypoint (4): 3.29161427437
-       Distance to waypoint (4): 3.63454331996
-       Distance to waypoint (4): 2.89070828637
-       >>> Reached Command #4
-       Distance to waypoint (5): 0.955857968149
-       >>> Reached Command #5
-       >>> Reached Command #6
-       >>> Reached Command #7
-       ...
-       >>> Reached Command #42
-       Distance to waypoint (42): 7.6983209285
-       ...
-       Distance to waypoint (43): 6.05247510021
-       >>> Reached Command #43
-       Distance to waypoint (43): 4.80180763387
-       >>> Reached Command #44
-       Distance to waypoint (44): 3.89880557617
+       Distance to waypoint (4): 4.91867197924
        ...
-       Distance to waypoint (45): 11.0865559753
-       >>> Reached Command #45
-       Distance to waypoint (46): 9.45419808223
        ...
-       Distance to waypoint (46): 13.2676499949
+       Distance to waypoint (35): 4.37309981133
+       >>> Reached Command #35
+       Distance to waypoint (36): 5.61829417257
+       >>> Reached Command #36
        Return to launch
        Close vehicle object
        Completed...
 
-   .. tip::
 
+   .. tip::
        It is more interesting to watch the example run on a map than the console. The topic :ref:`viewing_uav_on_map` 
        explains how to set up *Mission Planner* to view a vehicle running on the simulator (SITL).
 
@@ -132,22 +120,19 @@ In summary, after cloning the repository:
 How it works
 ============
 
-The example parses the **flight.tlog** file for position information. It then selects about 100
-points that are evenly spread across the log and uploads them as a mission. 
-
-For safety reasons, the altitude for the waypoints is set to 30 meters (irrespective of the recorded height).
-
 Getting the points
 ------------------
 
-The following simple function parses the tlog for points and then 
-returns 100 evenly points from the collected set.
+The example parses the **flight.tlog** file for position information. First we read all the points. 
+We then keep the first 99 points that are at least 3 metres separated from the preceding kept point.
+
+For safety reasons, the altitude for the waypoints is set to 30 meters (irrespective of the recorded height).
 
 .. code:: python
 
     def position_messages_from_tlog(filename):
         """
-        Given telemetry log, get a series of waypoints approximating the previous flight
+        Given telemetry log, get a series of wpts approximating the previous flight
         """
         # Pull out just the global position msgs
         messages = []
@@ -164,22 +149,36 @@ returns 100 evenly points from the collected set.
                 continue
             messages.append(m)
 
-            # Shrink the # of pts to be lower than the max # of wpts allowed by vehicle
+        # Shrink the number of points for readability and to stay within autopilot memory limits. 
+        # For coding simplicity we:
+        #   - only keep points that are with 3 metres of the previous kept point.
+        #   - only keep the first 100 points that meet the above criteria.
         num_points = len(messages)
-        max_points = 99
-        if num_points > max_points:
-            step = int(math.ceil((float(num_points) / max_points)))
-            shorter = [messages[i] for i in xrange(0, num_points, step)]
-            messages = shorter
-        return messages
+        keep_point_distance=3 #metres
+        kept_messages = []
+        kept_messages.append(messages[0]) #Keep the first message
+        pt1num=0
+        pt2num=1
+        while True:
+            #Keep the last point. Only record 99 points.
+            if pt2num==num_points-1 or len(kept_messages)==99:
+                kept_messages.append(messages[pt2num])
+                break
+            pt1 = LocationGlobalRelative(messages[pt1num].lat/1.0e7,messages[pt1num].lon/1.0e7,0)
+            pt2 = LocationGlobalRelative(messages[pt2num].lat/1.0e7,messages[pt2num].lon/1.0e7,0)
+            distance_between_points = get_distance_metres(pt1,pt2)
+            if distance_between_points > keep_point_distance:
+                kept_messages.append(messages[pt2num])
+                pt1num=pt2num
+            pt2num=pt2num+1
+
+        return kept_messages
 
 
 
 Setting the new waypoints
 -------------------------
 
-If necessary, the example then reduces the number of messages retrieved into a set that can fit on the vehicle (in this case 100 waypoints).
-
 The following code shows how the vehicle writes the received messages as commands (this part of the code is very similar to that
 shown in :ref:`example_mission_basic`):
 

docs/examples/flight_replay_example.png

@@ -86,14 +86,30 @@ def position_messages_from_tlog(filename):
             continue
         messages.append(m)
 
-        # Shrink the # of pts to be lower than the max # of wpts allowed by vehicle
+    # Shrink the number of points for readability and to stay within autopilot memory limits. 
+    # For coding simplicity we:
+    #   - only keep points that are with 3 metres of the previous kept point.
+    #   - only keep the first 100 points that meet the above criteria.
     num_points = len(messages)
-    max_points = 99
-    if num_points > max_points:
-        step = int(math.ceil((float(num_points) / max_points)))
-        shorter = [messages[i] for i in xrange(0, num_points, step)]
-        messages = shorter
-    return messages
+    keep_point_distance=3 #metres
+    kept_messages = []
+    kept_messages.append(messages[0]) #Keep the first message
+    pt1num=0
+    pt2num=1
+    while True:
+        #Keep the last point. Only record 99 points.
+        if pt2num==num_points-1 or len(kept_messages)==99:
+            kept_messages.append(messages[pt2num])
+            break
+        pt1 = LocationGlobalRelative(messages[pt1num].lat/1.0e7,messages[pt1num].lon/1.0e7,0)
+        pt2 = LocationGlobalRelative(messages[pt2num].lat/1.0e7,messages[pt2num].lon/1.0e7,0)
+        distance_between_points = get_distance_metres(pt1,pt2)
+        if distance_between_points > keep_point_distance:
+            kept_messages.append(messages[pt2num])
+            pt1num=pt2num
+        pt2num=pt2num+1
+
+    return kept_messages
 
 
 def arm_and_takeoff(aTargetAltitude):