From d47dcbdf771ea8ee6dddf9f9e4d097af7b5c75ff Mon Sep 17 00:00:00 2001
From: Akihiko Miyadera <miyadera@smilerobotics.com>
Date: Thu, 12 May 2022 20:19:13 +0900
Subject: [PATCH] Add settings parameter `stop_on_internal_limit`

---
 canopen_402/include/canopen_402/motor.h | 4 +++-
 canopen_402/src/motor.cpp               | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/canopen_402/include/canopen_402/motor.h b/canopen_402/include/canopen_402/motor.h
index 6404a712f..2006b23ab 100644
--- a/canopen_402/include/canopen_402/motor.h
+++ b/canopen_402/include/canopen_402/motor.h
@@ -296,7 +296,8 @@ class Motor402 : public MotorBase
     : MotorBase(name), status_word_(0),control_word_(0),
       switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
       monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
-      state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
+      state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5)),
+      stop_on_internal_limit_(settings.get_optional<bool>("stop_on_internal_limit", true))
     {
         storage->entry(status_word_entry_, 0x6041);
         storage->entry(control_word_entry_, 0x6040);
@@ -377,6 +378,7 @@ class Motor402 : public MotorBase
     const State402::InternalState switching_state_;
     const bool monitor_mode_;
     const boost::chrono::seconds state_switch_timeout_;
+    const bool stop_on_internal_limit_;
 
     canopen::ObjectStorage::Entry<uint16_t>  status_word_entry_;
     canopen::ObjectStorage::Entry<uint16_t >  control_word_entry_;
diff --git a/canopen_402/src/motor.cpp b/canopen_402/src/motor.cpp
index b6cc29d4a..628920ab1 100644
--- a/canopen_402/src/motor.cpp
+++ b/canopen_402/src/motor.cpp
@@ -397,7 +397,7 @@ bool Motor402::readState(LayerStatus &status, const LayerState &current_state){
         status.warn("mode does not match");
     }
     if(sw & (1<<State402::SW_Internal_limit)){
-        if(old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
+        if(!stop_on_internal_limit_ || old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
             status.warn("Internal limit active");
         }else{
             status.error("Internal limit active");