diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py index cc7deaeb4..d4edd521d 100644 --- a/socs/agents/acu/agent.py +++ b/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 diff --git a/socs/agents/acu/drivers.py b/socs/agents/acu/drivers.py index 90d535301..30f3665e2 100644 --- a/socs/agents/acu/drivers.py +++ b/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