From 1aa661d21821b4eebaa87bd6caabd2308baf55a6 Mon Sep 17 00:00:00 2001
From: Servando German Serrano <servando.german.serrano@gmail.com>
Date: Wed, 18 Aug 2021 14:24:03 +0100
Subject: [PATCH] add configurable homing timeout

Signed-off-by: Servando German Serrano <servando.german.serrano@gmail.com>
---
 canopen_402/include/canopen_402/motor.h |  6 ++-
 canopen_402/src/motor.cpp               | 57 ++++++++++++++++++++++++-
 2 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/canopen_402/include/canopen_402/motor.h b/canopen_402/include/canopen_402/motor.h
index 6404a712f..a92696fbd 100644
--- a/canopen_402/include/canopen_402/motor.h
+++ b/canopen_402/include/canopen_402/motor.h
@@ -261,6 +261,7 @@ class HomingMode: public Mode{
 public:
     HomingMode() : Mode(MotorBase::Homing) {}
     virtual bool executeHoming(canopen::LayerStatus &status) = 0;
+    virtual bool executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout) = 0;
 };
 
 class DefaultHomingMode: public HomingMode{
@@ -286,6 +287,7 @@ class DefaultHomingMode: public HomingMode{
     virtual bool write(OpModeAccesser& cw);
 
     virtual bool executeHoming(canopen::LayerStatus &status);
+    virtual bool executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout);
 };
 
 class Motor402 : public MotorBase
@@ -296,7 +298,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)),
+      homing_timeout_(settings.get_optional<unsigned int>("homing_timeout", 10))
     {
         storage->entry(status_word_entry_, 0x6041);
         storage->entry(control_word_entry_, 0x6040);
@@ -377,6 +380,7 @@ class Motor402 : public MotorBase
     const State402::InternalState switching_state_;
     const bool monitor_mode_;
     const boost::chrono::seconds state_switch_timeout_;
+    const boost::chrono::seconds homing_timeout_;
 
     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..8fc43888b 100644
--- a/canopen_402/src/motor.cpp
+++ b/canopen_402/src/motor.cpp
@@ -242,6 +242,61 @@ bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
     return error(status, "something went wrong while homing");
 }
 
+bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout) {
+    if(!homing_method_.valid()){
+        return error(status, "homing method entry is not valid");
+    }
+
+    if(homing_method_.get_cached() == 0){
+        return true;
+    }
+
+    time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
+    // ensure homing is not running
+    boost::mutex::scoped_lock lock(mutex_);
+    if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
+        return error(status, "could not prepare homing");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error before start");
+    }
+
+    execute_ = true;
+
+    // ensure start
+    if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
+        return error(status, "homing did not start");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error at start");
+    }
+
+    time_point finish_time = get_abs_time(homing_timeout); //
+
+    // wait for attained
+    if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
+        return error(status, "homing not attained");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error during process");
+    }
+
+    // wait for motion stop
+    if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
+        return error(status, "homing did not stop");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error during stop");
+    }
+
+    if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
+        execute_ = false;
+        return true;
+    }
+
+    return error(status, "something went wrong while homing");
+}
+
 bool Motor402::setTarget(double val){
     if(state_handler_.getState() == State402::Operation_Enable){
         boost::mutex::scoped_lock lock(mode_mutex_);
@@ -504,7 +559,7 @@ void Motor402::handleInit(LayerStatus &status){
         return;
     }
 
-    if(!homing->executeHoming(status)){
+    if(!homing->executeHoming(status, homing_timeout_)){
         status.error("Homing failed");
         return;
     }