HWP ACU checks for spin-up

socs/agents/hwp_supervisor/agent.py

@@ -138,6 +138,58 @@ def update(self):
         }
 
 
+@dataclass
+class ACUState:
+    """
+    Class containg ACU state information.
+
+    Args
+    ------
+    instance_id : str
+        Instance ID of ACU agent
+    min_el : float
+        Minimum elevation allowed before restricting spin-up [deg]
+    max_el : float
+        Maximum elevation allowed before restricting spin-up [deg]
+    max_time_since_update : float
+        Maximum time since last update before restricting spin-up[sec]
+
+    Attributes
+    ------------
+    el_current_position : float
+        Current el position [deg]
+    el_commanded_position : float
+        Commanded el position [deg]
+    el_current_velocity : float
+        Current el velocity [deg/s]
+    last_updated : float
+        Time of last update [sec]
+    """
+    instance_id: str
+    min_el: float
+    max_el: float
+    max_time_since_update: float
+
+    el_current_position: Optional[float] = None
+    el_commanded_position: Optional[float] = None
+    el_current_velocity: Optional[float] = None
+    last_updated: Optional[float] = None
+
+    def update(self):
+        op = get_op_data(self.instance_id, 'monitor')
+        if op['status'] != 'ok':
+            return
+
+        d = op['data'].get("StatusDetailed")
+        if d is None:
+            return
+
+        self.el_current_position = d['Elevation current position']
+        self.el_commanded_position = d['Elevation commanded position']
+        self.el_current_velocity = d['Elevation current velocity']
+        self.last_updated = time.time()
+
+
 @dataclass
 class HWPState:
     temp: Optional[float] = None
@@ -171,6 +223,8 @@ class HWPState:
     gripper_iboot: Optional[IBootState] = None
     driver_iboot: Optional[IBootState] = None
 
+    acu: Optional[ACUState] = None
+
     @classmethod
     def from_args(cls, args: argparse.Namespace):
         self = cls(
@@ -182,6 +236,8 @@ def from_args(cls, args: argparse.Namespace):
             self.gripper_iboot = IBootState(args.gripper_iboot_id, args.gripper_iboot_outlets)
         if args.driver_iboot_id is not None:
             self.driver_iboot = IBootState(args.driver_iboot_id, args.driver_iboot_outlets)
+        if args.acu_instance_id is not None:
+            self.acu = ACUState(args.acu_instance_id, args.acu_min_el, args.acu_max_time_since_update)
         return self
 
     def _update_from_keymap(self, op, keymap):
@@ -720,7 +776,20 @@ def query_pid_state():
                 self.log.info("pid state: {data}", data=data)
                 return data
 
+            def check_acu_ok_for_spinup():
+                acu = hwp_state.acu
+                if acu is not None:
+                    if acu.last_updated is None:
+                        raise RuntimeError(f"No ACU data has been received from instance-id {acu.instance_id}")
+                    tdiff = time.time() - acu.last_updated
+                    if tdiff < acu.max_time_since_update:
+                        raise RuntimeError(f"ACU state has not been updated in {tdiff} sec")
+                    if not (acu.min_el <= acu.el_current_position <= acu.max_el):
+                        raise RuntimeError(f"ACU elevation is {acu.el_current_pos} deg, "
+                                           f"outside of allowed range ({acu.min_el}, {acu.max_el})")
+
             if isinstance(state, ControlState.PIDToFreq):
+                check_acu_ok_for_spinup()
                 self.run_and_validate(clients.pid.set_direction,
                                       kwargs={'direction': state.direction})
                 self.run_and_validate(clients.pid.declare_freq,
@@ -813,6 +882,8 @@ def query_pid_state():
                     self.action.set_state(ControlState.Done(success=True))
 
             elif isinstance(state, ControlState.ConstVolt):
+                if state.voltage > 0:
+                    check_acu_ok_for_spinup()
                 self.run_and_validate(clients.pmx.set_on)
                 self.run_and_validate(clients.pid.set_direction,
                                       kwargs={'direction': state.direction})
@@ -1457,6 +1528,23 @@ def make_parser(parser=None):
         '--gripper-iboot-outlets', nargs='+', type=int,
         help="Outlets for gripper iboot power")
 
+    pgroup.add_argument(
+        '--acu-instance-id', help="Instance ID for the ACU agent"
+    )
+    pgroup.add_argument(
+        '--acu-min-el', help="Min elevation before restricting HWP spin up",
+        default=48.0
+    )
+    pgroup.add_argument(
+        '--acu-max-el', help="Min elevation before restricting HWP spin up",
+        default=90.0
+    )
+    pgroup.add_argument(
+        '--acu-max-time-since-update',
+        help="Max amount of time since last ACU update before restricting spin-up",
+        default=30.0,
+    )
+
     pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw",
                         help="Whether the PID 'forward' direction is cw or ccw")
     return parser