diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index dda96847..8e845b02 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -315,6 +315,11 @@ BOOL Ros_Controller_IsAlarm() || (g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ALARM_USER]!=0) ); } +BOOL Ros_Controller_IsMajorAlarm() +{ + return ((g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ALARM_MAJOR] != 0)); +} + BOOL Ros_Controller_IsError() { return ((g_Ros_Controller.ioStatus[IO_ROBOTSTATUS_ERROR]!=0)); diff --git a/src/ControllerStatusIO.h b/src/ControllerStatusIO.h index 0b95a5a2..255928d7 100644 --- a/src/ControllerStatusIO.h +++ b/src/ControllerStatusIO.h @@ -111,6 +111,7 @@ extern void Ros_Controller_StatusInit(); extern BOOL Ros_Controller_StatusRead(USHORT ioStatus[IO_ROBOTSTATUS_MAX]); extern BOOL Ros_Controller_IoStatusUpdate(); extern BOOL Ros_Controller_IsAlarm(); +extern BOOL Ros_Controller_IsMajorAlarm(); extern BOOL Ros_Controller_IsError(); extern BOOL Ros_Controller_IsPlay(); extern BOOL Ros_Controller_IsTeach(); diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 195f0738..3897c0d7 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -71,6 +71,8 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_TRAJ_MODE_ACTIVE_STR; case MOTION_NOT_READY_NOT_CONT_CYCLE_MODE: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_NOT_CONT_CYCLE_MODE_STR; + case MOTION_NOT_READY_MAJOR_ALARM: + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM_STR; default: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_UNSPECIFIED_STR; } diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index b1ae1325..366a7ded 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -32,6 +32,7 @@ typedef enum MOTION_NOT_READY_OTHER_PROGRAM_RUNNING = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_PROGRAM_RUNNING, MOTION_NOT_READY_OTHER_TRAJ_MODE_ACTIVE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_OTHER_TRAJ_MODE_ACTIVE, MOTION_NOT_READY_NOT_CONT_CYCLE_MODE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_NOT_CONT_CYCLE_MODE, + MOTION_NOT_READY_MAJOR_ALARM = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM, } MotionNotReadyCode; typedef enum diff --git a/src/ServiceResetError.c b/src/ServiceResetError.c index b78588e8..3b1ed316 100644 --- a/src/ServiceResetError.c +++ b/src/ServiceResetError.c @@ -83,7 +83,15 @@ void Ros_ServiceResetError_Trigger(const void* request_msg, void* response_msg) } } - // Check for condition that can be fixed remotely + // Major alarms cannot be reset remotely + if (Ros_Controller_IsMajorAlarm()) + { + rosidl_runtime_c__String__assign(&response->message, "Major alarm active. Cannot be reset. Check teach pendant"); + response->result_code.value = MOTION_NOT_READY_MAJOR_ALARM; + goto DONE; + } + + // Other alarm types can be reset remotely if (Ros_Controller_IsAlarm()) { // Reset alarm