Acu add sin el nod

socs/agents/acu/agent.py

@@ -362,6 +362,11 @@ def __init__(self, agent, acu_config='guess',
                                self._simple_process_stop,
                                blocking=False,
                                startup=False)
+        agent.register_process('generate_el_nod',
+                               self.generate_el_nod,
+                               self._simple_process_stop,
+                               blocking=False,
+                               startup=False)
         agent.register_process('idle_reset',
                                self.idle_reset,
                                self._simple_process_stop,
@@ -2446,6 +2451,155 @@ def generate_scan(self, session, params):
                                              'event': 2}})
         return ret_val
 
+    @ocs_agent.param('el_depth', type=float)
+    @ocs_agent.param('el_freq', type=float, default=None)
+    @ocs_agent.param('num_nods', type=int, default=None)
+    @ocs_agent.param('start_time', type=float, default=None)
+    @ocs_agent.param('step_time', type=float, default=0.1)
+    @inlineCallbacks
+    def generate_el_nod(self, session, params):
+        """generate_el_nod(el_depth, el_freq=None, num_nods=None, \
+                           start_time=None, step_time=0.1)
+
+        **Process** - Scan generator. Generates sinusoidal elevation nods
+        at the current azimuth and elevation of the platform. Nods are generated
+        with frequency equal to el_freq and amplitude equal to el_depth.
+        For a given el_depth, the platform will nod from the current elevation to
+        current elevation + el_depth.
+
+        This function publishes scan_params to the feed with scan_type = 4 .
+
+        Parameters:
+            el_depth (float): The number of degrees from the current
+                el to nod to. Can be negative.
+                Positive el_depth nods upwards, downwards for negative el_depth.
+            el_freq (float or None): frequency of the elevation nods.
+                If None, scan_params['el_freq'] is used.
+            num_nods (int or None): if not None, limits the nods to
+                the specified number of sinusoidal nods. The
+                process will exit without error once that has
+                completed.
+            start_time (float or None): a unix timestamp giving the
+                time at which the scan should begin.  The default is
+                None, which means the scan will start immediately.
+            step_time (float): time, in seconds, between points on the
+                constant-velocity parts of the motion.  The default is
+                0.1 seconds.
+
+        Notes:
+          Note that all parameters are optional except for
+          el_depth. If only the el_depth is passed, the Process will nod with that
+          depth and the el_freq specified in the scan_params (if it is specified)
+          indefinitely (until Process .stop method is called).
+          The azimuth axis held in Stop while the elevation nods are executed.
+
+        """
+        init_time = time.time()  # for params feed.
+
+        if self._get_sun_policy('motion_blocked'):
+            return False, "Motion blocked; Sun avoidance in progress."
+
+        self.log.info('User scan params: {params}', params=params)
+
+        az = self.data['status']['summary']['Azimuth_current_position']
+        el = self.data['status']['summary']['Elevation_current_position']
+
+        el_depth = params['el_depth']
+
+        el_endpoint1 = el
+        el_endpoint2 = el + el_depth
+
+        # Params with defaults configured ...
+        el_freq = params['el_freq']
+        if el_freq is None:
+            el_freq = self.scan_params['el_freq']
+
+        if abs(el_depth) < 0.1:
+            raise ValueError("El depth amplitude must be greater than 0.1 for el nod!")
+
+        if el_freq < 0.001:
+            raise ValueError("El freq must be greater than 0.001 Hz for el nod!")
+
+        # If requested el is just outside acceptable range, tweak it in.
+        _f, _ = self._get_limit_func('elevation')
+        el_endpoint1, _untweaked_el = _f(el_endpoint1), el_endpoint1
+        if abs(el_endpoint1 - _untweaked_el) > 0.1:
+            return False, "Current elevation (%.4f) is well outside limits." % _untweaked_el
+
+        scan_params = {k: params.get(k) for k in [
+            'num_nods', 'start_time', 'step_time']
+            if params.get(k) is not None}
+        step_time = 0.1 if 'step_time' not in scan_params.keys() else scan_params['step_time']
+
+        self.log.info('The scan_params: {scan_params}', scan_params=scan_params)
+
+        ok, msg = self._check_nod_sunsafe(az, el_endpoint1, el_endpoint2)
+        if ok:
+            self.log.info('Sun safety check: {msg}', msg=msg)
+        else:
+            self.log.error('Sun safety check fails: {msg}', msg=msg)
+            return False, 'Scan is not Sun Safe.'
+
+        # Clear faults.
+        self.log.info('Clearing faults to prepare for motion.')
+        yield self.acu_control.clear_faults()
+        yield dsleep(1)
+
+        # Verify we're good to move
+        ok, msg = yield self._check_ready_motion(session)
+        if not ok:
+            return False, msg
+
+        # Also validate the scan generally -- need to be movable in el.
+        hwp_safe, msg = yield self._check_hwpsafe(el_endpoint1, el_endpoint2, axes=['el'])
+        if not hwp_safe:
+            msg = f'El-nod not permitted: {msg}'
+            self.log.info(msg)
+            return False, msg
+
+        # Prepare the point generator.
+        free_form = True
+        track_axes = ['el']
+        g = sh.generate_sin_el_nod(az=az, el_endpoint1=el_endpoint1,
+                                   el_endpoint2=el_endpoint2,
+                                   el_freq=el_freq,
+                                   **scan_params)
+
+        scan_params_bundle = {'session_id': session.session_id,
+                              'schema': 1,
+                              'event': 1,
+                              'init_time': init_time,
+                              }
+
+        scan_params_bundle.update({
+            'az1': az,
+            'az2': az,
+            'az_vel': 0,
+            'az_accel': 0,
+            'el1': el_endpoint1,
+            'el2': el_endpoint2,
+            'el_freq': el_freq,
+            'type': 4,
+            'turnaround_type': 0,
+            'track_axes': ','.join(track_axes),
+        })
+
+        self.agent.publish_to_feed('scan_params',
+                                   {'timestamp': time.time(),
+                                    'block_name': 'info',
+                                    'data': scan_params_bundle})
+
+        ret_val = (yield self._run_track(
+            session=session, point_gen=g, step_time=step_time,
+            track_axes=track_axes, free_form=free_form, unabort_failure=True))
+
+        self.agent.publish_to_feed('scan_params',
+                                   {'timestamp': time.time(),
+                                    'block_name': 'exit',
+                                    'data': {'session_id': session.session_id,
+                                             'event': 2}})
+        return ret_val
+
     @inlineCallbacks
     def _run_track(self, session, point_gen, step_time, stop_accel=0.5, track_axes=['az'],
                    point_batch_count=None, free_form=False, unabort_failure=False):
@@ -2551,7 +2705,7 @@ def _run_track(self, session, point_gen, step_time, stop_accel=0.5, track_axes=[
             last_mode = None
             last_upload_az = None
             start_time = time.time()
-            got_progtrack = False
+            got_progtrack = {ax: False for ax in track_axes}
             faults = {}
             got_points_in = False
             first_upload_time = None
@@ -2573,7 +2727,7 @@ def _run_track(self, session, point_gen, step_time, stop_accel=0.5, track_axes=[
                 # points but ACU is quietly dumping them because the
                 # vel is too high.
                 got_points_in = got_points_in \
-                    or (got_progtrack and free_positions < FULL_STACK)
+                    or (any(got_progtrack.values()) and free_positions < FULL_STACK)
 
                 if last_mode != mode:
                     self.log.info(f'scan mode={mode}, line_buffer={len(point_prov)}, track_free={free_positions}')
@@ -2585,25 +2739,35 @@ def _run_track(self, session, point_gen, step_time, stop_accel=0.5, track_axes=[
                         faults[k] = True
 
                 if mode != 'abort':
-                    # Reasons we might decide to abort ...
-                    if current_modes['Az'] == 'ProgramTrack':
-                        got_progtrack = True
-                    else:
-                        if got_progtrack:
-                            self.log.warn('Unexpected exit from ProgramTrack mode!')
-                            if mode == 'stop':
-                                prog_track_err = True
-                            mode = 'abort'
-                        elif now - start_time > MAX_PROGTRACK_SET_TIME:
-                            self.log.warn('Failed to set ProgramTrack mode in a timely fashion.')
-                            mode = 'abort'
+                    # Reasons we might decide to abort or stop ...
+
+                    # First we'll check that each axis we expect to be in ProgramTrack mode,
+                    # is indeed still in ProgramTrack mode.
+                    for ax in track_axes:
+                        if current_modes[ax.capitalize()] == 'ProgramTrack':
+                            got_progtrack[ax] = True
+                        else:
+                            if got_progtrack[ax]:
+                                self.log.warn(f'Unexpected exit from ProgramTrack mode on {ax} axis!')
+                                if mode == 'stop':
+                                    prog_track_err = True
+                                mode = 'abort'
+                            elif now - start_time > MAX_PROGTRACK_SET_TIME:
+                                self.log.warn(f'Failed to set ProgramTrack mode in a timely fashion on {ax} axis.')
+                                mode = 'abort'
+
+                    # If we've attempted to upload points does the ACU actually have them?
                     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'
+
+                    # Have we left Remote mode?
                     if current_modes['Remote'] == 0:
                         self.log.warn('ACU no longer in remote mode!')
                         mode = 'abort'
+
+                    # Have we requested a stop?
                     if session.status == 'stopping' and mode not in ['stop', 'abort']:
                         mode = 'stop'
                         stop_message = 'User-requested stop.'
@@ -3222,6 +3386,30 @@ def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az):
 
         return safe, msg
 
+    def _check_nod_sunsafe(self, az, el1, el2):
+        """This will return True if active avoidance is disabled.  If active
+        avoidance is enabled, then it will only return true if the
+        planned El-nod seems to currently be sun-safe.
+
+        """
+        if not self._get_sun_policy('sunsafe_moves'):
+            return True, 'Sun-safety checking is not enabled.'
+
+        if not self._get_sun_policy('map_valid'):
+            return False, 'Sun Safety Map not computed or stale; run the monitor_sun process.'
+
+        n = max(2, int(np.ceil((abs(el2 - el1)) / 1.)))
+        els = np.linspace(el1, el2, n)
+
+        info = self.sun.check_trajectory(els * 0 + az, els)
+        safe = info['sun_time'] >= self.sun_params['policy']['min_sun_time']
+        if safe:
+            msg = 'El-nod is safe for %.1f hours' % (info['sun_time'] / 3600)
+        else:
+            msg = 'El-nod will be unsafe in %.1f hours' % (info['sun_time'] / 3600)
+
+        return safe, msg
+
     def _get_sunsafe_moves(self, target_az, target_el):
         """Given a target position, find a Sun-safe way to get there.  This
         will either be a direct move, or else an ordered slew in az

socs/agents/acu/drivers.py

@@ -999,6 +999,111 @@ def generate_type2_scan(az_endpoint1, az_endpoint2, az_speed,
                                turnaround_method=turnaround_method)
 
 
+def generate_sin_el_nod(az, el_endpoint1, el_endpoint2,
+                        el_freq=.15,
+                        num_nods=None,
+                        start_time=None,
+                        wait_to_start=10.,
+                        step_time=0.05):
+    """Python generator to produce times, azimuth and elevation positions,
+    azimuth and elevation velocities, azimuth and elevation flags for
+    arbitrarily long sin el nods.
+
+    Parameters:
+        az (float): azimuth position to do the sin el nod. Azimuth position
+            stays constant throughout the sin el nod.
+        el_endpoint1 (float): elevation endpoint for the el nod start.
+        el_endpoint2 (float): second elevation endpoint of the scan.
+            Can be greater than el_endpoint1 for a positive el nod or
+            less than el_endpoint2 for a negative el nod.
+        el_freq(float): frequency of the elevation nods in Hz. Defaults to 0.15.
+        num_nods (int or None): if not None, limits the points
+          returned to the specified number of el nods (one nod is one full sine wave).
+        start_time (float or None): a ctime at which to start the scan.
+            Default is None, which is interpreted as starting now +
+            wait_to_start.
+        wait_to_start (float): number of seconds to wait between
+            start_time and when the scan actually starts. Default is 10 seconds.
+        step_time (float): suggested time between points on the constant-velocity
+            parts of the motion. Actual value will be computed to fit evenly into
+            a single nod period. Default value is 0.05 seconds, and that's also
+            the minimum accepted value.
+
+    Yields:
+        points (list): a list of TrackPoint objects.  Raises
+          StopIteration once exit condition, if defined, is met.
+
+    """
+
+    MIN_STEP_TIME = 0.05  # in seconds.
+    MIN_POINTS_PER_NOD = 16
+    # Get el throw.
+    el_throw = (el_endpoint2 - el_endpoint1) / 2
+    el_cent = (el_endpoint1 + el_endpoint2) / 2.
+
+    if start_time is None:
+        t0 = time.time() + wait_to_start
+    else:
+        t0 = start_time
+
+    t = 0
+    el = el_endpoint1
+
+    # Ensure the step_time isn't too low.
+    if step_time < MIN_STEP_TIME:
+        step_time = MIN_STEP_TIME
+
+    nod_period = 1. / el_freq
+    points_per_nod = round(nod_period / step_time)
+
+    # Tweak points per nod to the closest multiple of 4.
+    # For clean sine/cos waves.
+    points_per_nod = round(points_per_nod / 4) * 4
+
+    # Ensure we have the minumum number of points
+    if points_per_nod < MIN_POINTS_PER_NOD:
+        points_per_nod = MIN_POINTS_PER_NOD
+
+    step_time = nod_period / points_per_nod  # Divide nod into points_per_nod points.
+
+    def check_completed_nods():
+        return num_nods is None or num_completed_nods < num_nods
+
+    def get_el(_t):
+        return (el_cent - el_throw * np.cos(_t * el_freq * 2 * np.pi),
+                el_throw * el_freq * 2 * np.pi * np.sin(_t * el_freq * 2 * np.pi))
+
+    # Set up templates for the ts, els, and el_vels for each nod.
+    # Each nod is identical in el and el_vel,
+    # we just need to update the times as we complete nods.
+    template_nod_ts = step_time * np.arange(points_per_nod)
+    template_nod_els, template_nod_el_vels = get_el(template_nod_ts)
+
+    # Upload full nods until we've completed num_nods.
+    num_completed_nods = 0
+    while check_completed_nods():
+        # Create a point_block for the entire nod using our templates.
+        point_block = [TrackPoint(timestamp=nod_t + t + t0,
+                                  az=az, el=el, az_vel=0, el_vel=el_vel,
+                                  az_flag=0, el_flag=1, group_flag=1)
+                       for nod_t, el, el_vel in
+                       zip(template_nod_ts, template_nod_els, template_nod_el_vels)]
+
+        # Set the group flag of the last point to 0 so the entire nod gets uploaded at once.
+        point_block[-1].group_flag = 0
+
+        # Update the time.
+        t += 1. / el_freq
+        yield point_block
+
+        num_completed_nods += 1
+
+    # Yield one final point with 0 velocities.
+    yield [TrackPoint(timestamp=t + t0,
+                      az=az, el=el, az_vel=0, el_vel=0,
+                      az_flag=0, el_flag=1, group_flag=0)]
+
+
 def plan_scan(az_end1, az_end2, el, v_az=1, a_az=1, az_start=None,
               scan_type=1):
     """Determine some important parameters for running a ProgramTrack