ACU Agent: generate_scan waits for settling, before exiting

socs/agents/acu/agent.py

@@ -1996,24 +1996,30 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
 
             # Values for mode are:
             # - 'go' -- keep uploading points (unless there are no more to upload).
-            # - 'stop' -- do not request more points from generator; finish the ones you have.
-            # - 'abort' -- do not upload more points; exit loop and clear stack.
+            # - 'stop' -- do not request more points from generator;
+            #   finish the ones that are already in "points", let the stack empty,
+            #   and wait for settling condition.
+            # - 'abort' -- do not upload more points; exit loop with error; wait
+            #   a few seconds and clear the stack.
             mode = 'go'
 
             lines = []
             last_mode = None
-            was_graceful_exit = True
+            last_upload_az = None
             start_time = time.time()
             got_progtrack = False
             faults = {}
             got_points_in = False
             first_upload_time = None
+            wait_stop_timeout = None
 
             while True:
                 now = time.time()
                 current_modes = {'Az': self.data['status']['summary']['Azimuth_mode'],
                                  'El': self.data['status']['summary']['Elevation_mode'],
                                  'Remote': self.data['status']['platform_status']['Remote_mode']}
+                az_state = {'pos': self.data['status']['summary']['Azimuth_current_position'],
+                            'vel': self.data['status']['summary']['Azimuth_current_velocity']}
                 free_positions = self.data['status']['summary']['Free_upload_positions']
 
                 # Use this var to detect case where we're uploading
@@ -2039,21 +2045,20 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
                         if got_progtrack:
                             self.log.warn('Unexpected exit from ProgramTrack mode!')
                             mode = 'abort'
-                            was_graceful_exit = False
                         elif now - start_time > MAX_PROGTRACK_SET_TIME:
                             self.log.warn('Failed to set ProgramTrack mode in a timely fashion.')
                             mode = 'abort'
-                            was_graceful_exit = False
                     if not got_points_in and (first_upload_time is not None) \
                        and (now - first_upload_time > 10):
                         self.log.warn('ACU seems to be dumping our track. Vel too high?')
                         mode = 'abort'
                     if current_modes['Remote'] == 0:
                         self.log.warn('ACU no longer in remote mode!')
                         mode = 'abort'
-                        was_graceful_exit = False
                     if session.status == 'stopping':
-                        mode = 'abort'
+                        mode = 'stop'
+                        stop_message = 'User-requested stop.'
+                        lines = []
 
                 if mode == 'abort':
                     lines = []
@@ -2067,22 +2072,24 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
                     # after "grabbing the minimum batch", below, there
                     # is still >= 1 line left.  The lines-is-empty
                     # check is used to decide we're done.
-                    while mode == 'go' and (len(lines) <= new_line_target or lines[-1][0] != 0):
+                    while mode == 'go' and (len(lines) <= new_line_target or lines[-1].group_flag != 0):
                         try:
                             lines.extend(next(point_gen))
                         except StopIteration:
                             mode = 'stop'
+                            stop_message = 'Stop due to end of the planned track.'
 
                     # Grab the minimum batch
                     upload_lines, lines = lines[:new_line_target], lines[new_line_target:]
 
                     # If the last line has a "group" flag, keep transferring lines.
-                    while len(lines) and len(upload_lines) and upload_lines[-1][0] != 0:
+                    while len(lines) and len(upload_lines) and upload_lines[-1].group_flag != 0:
                         upload_lines.append(lines.pop(0))
 
                     if len(upload_lines):
                         # Discard the group flag and upload all.
-                        text = ''.join([line for _flag, line in upload_lines])
+                        text = sh.get_track_points_text(
+                            upload_lines, timestamp_offset=3, text_block=True)
                         for attempt in range(5):
                             _dt = time.time()
                             try:
@@ -2098,24 +2105,39 @@ def _run_track(self, session, point_gen, step_time, azonly=False,
                             raise RuntimeError('Upload fail.')
                         if first_upload_time is None:
                             first_upload_time = time.time()
+                        last_upload_az = upload_lines[-1].az
 
                 if len(lines) == 0 and free_positions >= FULL_STACK - 1:
-                    break
+                    if mode == 'stop':
+                        if wait_stop_timeout is None:
+                            self.log.info('Stack is empty; waiting for settling...')
+                            wait_stop_timeout = now + 20.
+                        elif now > wait_stop_timeout:
+                            self.log.warn('Graceful stop condition not met in a timely fashion.')
+                            mode = 'abort'
+                        # Await safe exit condition.
+                        pos_ok = last_upload_az is None or (
+                            abs(az_state['pos'] - last_upload_az) < 0.01)
+                        vel_ok = abs(abs(az_state['vel']) < .01)
+                        if pos_ok and vel_ok:
+                            break
+                    else:
+                        self.log.warn('Somehow ran out of points!')
+                        break
 
                 yield dsleep(LOOP_STEP)
 
             # Go to Stop mode?
             # yield self.acu_control.stop()
 
-            # Clear the stack, but wait a bit or it can cause a fault.
-            # Yes, sometimes you have to wait a very long time ...
-            yield dsleep(10)
+            # Wait a couple more seconds and clear the stack.
+            yield dsleep(2)
             yield self.acu_control.http.Command('DataSets.CmdTimePositionTransfer',
                                                 'Clear Stack')
 
-        if not was_graceful_exit:
+        if mode == 'abort':
             return False, 'Problems during scan'
-        return True, 'Scan ended cleanly'
+        return True, f'Scan ended. {stop_message}'
 
     #
     # Sun Safety Monitoring and Active Avoidance

socs/agents/acu/drivers.py

@@ -2,6 +2,7 @@
 import datetime
 import math
 import time
+from dataclasses import dataclass
 
 import numpy as np
 
@@ -13,6 +14,72 @@
 MIN_GROUP_NEW_LEG = 4
 
 
+def _progtrack_format_time(timestamp):
+    fmt = '%j, %H:%M:%S'
+    return (time.strftime(fmt, time.gmtime(timestamp))
+            + '{:.6f}'.format(timestamp % 1.)[1:])
+
+
+@dataclass
+class TrackPoint:
+    #: Timestamp of the point (unix timestamp)
+    timestamp: float
+
+    #: Azimuth (deg).
+    az: float
+
+    #: Elevation (deg).
+    el: float
+
+    #: Azimuth velocity (deg/s).
+    az_vel: float
+
+    #: Elevation velocity (deg/s).
+    el_vel: float
+
+    #: Az flag: 0 if stationary, 1 if non-final point of const-vel
+    #: scan segment; 2 if final point of const-vel segment.
+    az_flag: int = 0
+
+    #: El flag: like az_flag but for el.
+    el_flag: int = 0
+
+    #: If 1, indicates that once this point is uploaded the next point
+    #: in sequence also needs to be soon uploaded.  Used at start of a
+    #: new const-vel scan segment.
+    group_flag: int = 0
+
+
+def get_track_points_text(tpl, timestamp_offset=None, with_group_flag=False,
+                          text_block=False):
+    """Get a list of ProgramTrack lines for upload to ACU.
+
+    Args:
+      tpl (list): list of TrackPoint to convert.
+      timestamp_offset (float): offset to add to all timestamps
+        before rendering (defaults to 0).
+      with_group_flag (bool): If True return each line as
+        (group_flag, text).
+      text_block (bool): If True, return all lines joined together
+        into a single string.
+
+    """
+    if timestamp_offset is None:
+        timestamp_offset = 0
+    fmted_times = [_progtrack_format_time(p.timestamp + timestamp_offset)
+                   for p in tpl]
+    all_lines = [('{t}; {p.az:.6f}; {p.el:.6f}; {p.az_vel:.4f}; '
+                  '{p.el_vel:.4f}; {p.az_flag}; {p.el_flag}\r\n')
+                 .format(p=p, t=t)
+                 for p, t in zip(tpl, fmted_times)]
+
+    if text_block:
+        return ''.join(all_lines)
+    if with_group_flag:
+        all_lines = [(p.group_flag, line) for p, line in zip(tpl, all_lines)]
+    return all_lines
+
+
 def constant_velocity_scanpoints(azpts, el, azvel, acc, ntimes):
     """
     Produces lists of times, azimuths, elevations, azimuthal velocities,
@@ -225,8 +292,7 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed,
                                     batch_size=500,
                                     az_start='mid_inc',
                                     az_first_pos=None,
-                                    az_drift=None,
-                                    ptstack_fmt=True):
+                                    az_drift=None):
     """Python generator to produce times, azimuth and elevation positions,
     azimuth and elevation velocities, azimuth and elevation flags for
     arbitrarily long constant-velocity azimuth scans.
@@ -270,11 +336,10 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed,
         az_drift (float): The rate (deg / s) at which to shift the
             scan endpoints in time.  This can be used to better track
             celestial sources in targeted scans.
-        ptstack_fmt (bool): determine whether values are produced with the
-            necessary format to upload to the ACU. If False, this function will
-            produce lists of time, azimuth, elevation, azimuth velocity,
-            elevation velocity, azimuth flags, and elevation flags. Default is
-            True.
+
+    Yields:
+        points (list): a list of TrackPoint objects.  Raises
+          StopIteration once exit condition, if defined, is met.
 
     """
     def get_target_az(current_az, current_t, increasing):
@@ -363,16 +428,13 @@ def check_num_scans():
     i = 0
     while i < stop_iter and check_num_scans():
         i += 1
-        point_block = [[], [], [], [], [], [], [], []]
+        point_block = []
         for j in range(batch_size):
-            point_block[0].append(t + t0)
-            point_block[1].append(az)
-            point_block[2].append(el)
-            point_block[3].append(az_vel)
-            point_block[4].append(el_vel)
-            point_block[5].append(az_flag)
-            point_block[6].append(el_flag)
-            point_block[7].append(int(point_group_batch > 0))
+            point_block.append(TrackPoint(
+                timestamp=t + t0,
+                az=az, el=el, az_vel=az_vel, el_vel=el_vel,
+                az_flag=az_flag, el_flag=el_flag,
+                group_flag=int(point_group_batch > 0)))
 
             if point_group_batch > 0:
                 point_group_batch -= 1
@@ -436,18 +498,11 @@ def check_num_scans():
                 # Kill the velocity on the last point and exit -- this
                 # was recommended at LAT FAT for smoothly stopping the
                 # motion at end of program.
-                point_block[3][-1] = 0
-                point_block[4][-1] = 0
+                point_block[-1].az_vel = 0
+                point_block[-1].el_vel = 0
                 break
 
-        if ptstack_fmt:
-            yield ptstack_format(point_block[0], point_block[1],
-                                 point_block[2], point_block[3],
-                                 point_block[4], point_block[5],
-                                 point_block[6], point_block[7],
-                                 start_offset=3, absolute=True)
-        else:
-            yield tuple(point_block)
+        yield point_block
 
 
 def plan_scan(az_end1, az_end2, el, v_az=1, a_az=1, az_start=None):

tests/agents/test_acu_agent.py

@@ -1,4 +1,5 @@
 from socs.agents.acu import avoidance as av
+from socs.agents.acu import drivers
 from socs.agents.acu.agent import ACUAgent  # noqa: F401
 
 
@@ -61,3 +62,13 @@ def test_avoidance():
     assert path is not None
     path = sun.find_escape_paths(az0 + 10, el0)
     assert path is not None
+
+
+def test_tracks():
+    # Basic function testing.
+    g = drivers.generate_constant_velocity_scan(
+        60, 80, 1, 1, 50, 50,
+        start_time=1800000000)
+    points = next(iter(g))
+    drivers.get_track_points_text(points, text_block=True,
+                                  timestamp_offset=3)