diff --git a/socs/agents/hwp_pid/agent.py b/socs/agents/hwp_pid/agent.py index 5381406d3..753613002 100644 --- a/socs/agents/hwp_pid/agent.py +++ b/socs/agents/hwp_pid/agent.py @@ -29,11 +29,18 @@ def parse_action_result(res): def get_pid_state(pid: pd.PID): - return { - "current_freq": pid.get_freq(), - "target_freq": pid.get_target(), - "direction": pid.get_direction(), - } + state_func = {'current_freq': pid.get_freq, + 'target_freq': pid.get_target, + 'direction': pid.get_direction} + + return_dict = {'healthy': True} + for name, func in state_func.items(): + resp = func() + if resp.msg_type == 'error': + return_dict['healthy'] = False + else: + return_dict[name] = resp.measure + return return_dict class Actions: @@ -88,7 +95,12 @@ def process(self, pid: pd.PID): @dataclass class GetState(BaseAction): def process(self, pid: pd.PID): - return get_pid_state(pid) + pid_state = get_pid_state(pid) + if pid_state['healthy']: + return pid_state + else: + print('Error getting state') + raise ValueError class HWPPIDAgent: @@ -119,6 +131,12 @@ def _get_data_and_publish(self, pid: pd.PID, session: ocs_agent.OpSession): data = {"timestamp": time.time(), "block_name": "HWPPID", "data": {}} pid_state = get_pid_state(pid) + if pid_state['healthy']: + session.degraded = False + else: + print('Warning: state monitor degraded') + session.degraded = True + data['data'].update(pid_state) session.data.update(pid_state) session.data['last_updated'] = time.time() diff --git a/socs/agents/hwp_pid/drivers/pid_controller.py b/socs/agents/hwp_pid/drivers/pid_controller.py index 8e82ff31d..b5a5dc6b3 100644 --- a/socs/agents/hwp_pid/drivers/pid_controller.py +++ b/socs/agents/hwp_pid/drivers/pid_controller.py @@ -5,6 +5,20 @@ from typing import Optional, Union +def retry_multiple_times(loops=3): + def dec_wrapper(func): + def inner(*args, **kwargs): + for i in range(loops): + try: + return func(*args, **kwargs) + except BaseException: + time.sleep(0.2) + print(f'Could not complete {func.__name__} after {loops} attempt(s)') + return DecodedResponse(msg_type='error', msg='Read Error') + return inner + return dec_wrapper + + @dataclass class DecodedResponse: msg_type: str @@ -221,6 +235,7 @@ def tune_freq(self): tune_params = [0.2, 63, 0] self.set_pid(tune_params) + @retry_multiple_times(loops=3) def get_freq(self): """Returns the current frequency of the CHWP. @@ -236,17 +251,19 @@ def get_freq(self): responses = [] responses.append(self.send_message("*X01")) decoded_resp = self.return_messages(responses)[0] - attempts = 3 - for attempt in range(attempts): - if self.verb: - print(responses) - print(decoded_resp) - if decoded_resp.msg_type == 'measure': - return decoded_resp.measure - elif decoded_resp.msg_type == 'error': - print(f"Error reading freq: {decoded_resp.msg}") - raise ValueError('Could not get current frequency') + if self.verb: + print(responses) + print(decoded_resp) + if decoded_resp.msg_type == 'measure': + return decoded_resp + elif decoded_resp.msg_type == 'error': + print(f"Error reading freq: {decoded_resp.msg}") + raise ValueError + else: + print("Unknown freq response") + raise ValueError + @retry_multiple_times(loops=3) def get_target(self): """Returns the target frequency of the CHWP. @@ -262,17 +279,19 @@ def get_target(self): responses = [] responses.append(self.send_message("*R01")) decoded_resp = self.return_messages(responses)[0] - attempts = 3 - for attempt in range(attempts): - if self.verb: - print(responses) - print(decoded_resp) - if decoded_resp.msg_type == 'read': - return decoded_resp.measure - elif decoded_resp.msg_type == 'error': - print(f"Error reading target: {decoded_resp.msg}") - raise ValueError('Could not get target frequency') + if self.verb: + print(responses) + print(decoded_resp) + if decoded_resp.msg_type == 'read': + return decoded_resp + elif decoded_resp.msg_type == 'error': + print(f"Error reading target: {decoded_resp.msg}") + raise ValueError + else: + print('Unknown target response') + raise ValueError + @retry_multiple_times(loops=3) def get_direction(self): """Get the current rotation direction. @@ -291,16 +310,17 @@ def get_direction(self): responses = [] responses.append(self.send_message("*R02")) decoded_resp = self.return_messages(responses)[0] - attempts = 3 - for attempt in range(attempts): - if self.verb: - print(responses) - print(decoded_resp) - if decoded_resp.msg_type == 'read': - return decoded_resp.measure - elif decoded_resp.msg_type == 'error': - print(f"Error reading direction: {decoded_resp.msg}") - raise ValueError('Could not get direction') + if self.verb: + print(responses) + print(decoded_resp) + if decoded_resp.msg_type == 'read': + return decoded_resp + elif decoded_resp.msg_type == 'error': + print(f"Error reading direction: {decoded_resp.msg}") + raise ValueError + else: + print('Unknown direction response') + raise ValueError def set_pid(self, params): """Sets the PID parameters of the controller. @@ -525,6 +545,8 @@ def _decode_read(string): """ end_string = string.split('\r')[-1] read_type = end_string[1:3] + if len(end_string) != 9: + return DecodedResponse(msg_type='error', msg='Unrecognized Read Length') # Decode target if read_type == '01': target = float(int(end_string[4:], 16) / 1000.) @@ -536,7 +558,7 @@ def _decode_read(string): else: return DecodedResponse(msg_type='read', msg='Direction = Forward', measure=0) else: - return DecodedResponse(msg_type='error', msg='Unrecognized Read') + return DecodedResponse(msg_type='error', msg='Unrecognized Read Type') @staticmethod def _decode_write(string): diff --git a/socs/testing/hwp_emulator.py b/socs/testing/hwp_emulator.py index 538d650c3..44b85cbf0 100644 --- a/socs/testing/hwp_emulator.py +++ b/socs/testing/hwp_emulator.py @@ -189,7 +189,7 @@ def process_pid_msg(self, data): elif cmd == "*X01": # Get frequency return f"X01{self.state.cur_freq:0.3f}" elif cmd == "*R01": # Get Target - return f"R01{PID._convert_to_hex(self.state.pid.freq_setpoint, 3)}" + return f"R0140{PID._convert_to_hex(self.state.pid.freq_setpoint, 3)}" elif cmd == "*R02": # Get Direction if self.state.pid.direction == "forward": return "R02400000"