CbParsleyDriver: several improvements for error handling and logging

modules/CbParsleyDriver/evse_board_support/evse_board_supportImpl.cpp

@@ -81,19 +81,28 @@ void evse_board_supportImpl::init() {
 
     this->mod->controller.on_ce_change.connect([&](const types::cb_board_support::CEState& ce_state) {
         std::scoped_lock lock(this->cp_mutex);
-        auto current_cp_state = cestate_to_cpstate(ce_state);
+        const types::cb_board_support::CEState previous_ce_state = this->ce_current_state;
 
-        // B0 is mapped to D; we need to ignore B0 and not forward it here
-        if (current_cp_state == types::cb_board_support::CPState::D)
+        if (this->ce_current_state != ce_state) {
+            EVLOG_info << "CE change detected: " << this->ce_current_state << " → " << ce_state;
+        } else {
+            EVLOG_debug << "CE change detected: " << this->ce_current_state << " → " << ce_state;
+        }
+        this->ce_current_state = ce_state;
+
+        // filter out uninitialized value (e.g. after reset)
+        if (ce_state == types::cb_board_support::CEState::PowerOn)
             return;
 
-        EVLOG_info << "CP state change from " << this->cp_current_state << " to " << current_cp_state;
-        this->cp_current_state = current_cp_state;
+        // we need to ignore B0 and not forward it here
+        if (ce_state == types::cb_board_support::CEState::B0)
+            return;
 
         // in case safety controller was in emergency state and EV is gone,
         // we have to reset safety controller with a disable -> enable toggle
-        if (current_cp_state == types::cb_board_support::CPState::A and this->mod->controller.is_emergency()) {
-            EVLOG_info << "recovering after safe state";
+        if (this->mod->controller.is_emergency() and previous_ce_state != types::cb_board_support::CEState::A and
+            ce_state == types::cb_board_support::CEState::A) {
+            EVLOG_info << "recovering after safe state (CE triggered)";
 
             // disable resets the controller
             this->mod->controller.disable();
@@ -102,8 +111,15 @@ void evse_board_supportImpl::init() {
             this->mod->controller.enable();
         }
 
+        auto new_cp_state = cestate_to_cpstate(ce_state);
+        EVLOG_info << "simulate CP change: " << this->cp_current_state << " → " << new_cp_state;
+        this->cp_current_state = new_cp_state;
+
+        if (new_cp_state == types::cb_board_support::CPState::PilotFault)
+            return;
+
         try {
-            const types::board_support_common::BspEvent tmp = cpstate_to_bspevent(current_cp_state);
+            const types::board_support_common::BspEvent tmp = cpstate_to_bspevent(new_cp_state);
             this->publish_event(tmp);
         } catch (std::runtime_error& e) {
             // should never happen, when all invalid states are handled correctly
@@ -194,6 +210,28 @@ void evse_board_supportImpl::init() {
             this->clear_error("evse_board_support/VendorWarning", module_str);
         }
     });
+
+    this->mod->controller.on_id_change.connect([&](const types::cb_board_support::IDState& id_state) {
+        // general note: logging of state changes is already done in MCS interface, so we don't double it here
+
+        // we use ID changes only to have a trigger to recover from emergency states: usually such a reset
+        // is done when EV is disconnected and we see a CE state change from X to A, but in case of
+        // CE malfunction, we might not see it and thus we would be stuck in this state; using ID here
+        // could help to recover
+        if (this->mod->controller.is_emergency() and
+            ((this->ce_current_state == types::cb_board_support::CEState::Invalid and
+              id_state == types::cb_board_support::IDState::NotConnected) or
+             (this->ce_current_state == types::cb_board_support::CEState::A and
+              id_state == types::cb_board_support::IDState::Connected))) {
+            EVLOG_info << "recovering after safe state (ID triggered)";
+
+            // disable resets the controller
+            this->mod->controller.disable();
+
+            // enable starts UART frame processing again
+            this->mod->controller.enable();
+        }
+    });
 }
 
 void evse_board_supportImpl::ready() {
@@ -223,14 +261,37 @@ void evse_board_supportImpl::handle_pwm_on(double& value) {
 }
 
 void evse_board_supportImpl::handle_cp_state_X1() {
-    EVLOG_info << "handle_cp_state_X1: setting new duty cycle of 100.0% (ignored)";
+    if (this->mod->controller.is_emergency() and this->cp_current_state == types::cb_board_support::CPState::C) {
+        EVLOG_info << "handle_cp_state_X1: setting new duty cycle of 100.0% (in safe state)";
+
+        // When safety controller has gone into safe state, then we raised an error to EvseManager.
+        // EvseManagers reaction is to instruct us to go to 100%. A typical EV would switch to
+        // state B then. Here, the safety controller already opened S_s_3 which results in EC.
+        // The safety controller _could_ report EC (or then B0) but in case of CE malfunction,
+        // this could also not happen. To satisfy EvseManager, we report that we see state B.
+        // This prevents loosing sync and the error "Timeout of 6 seconds reached, EV did not
+        // go back to state B after PWM was switched off. Powering off under load.".
+        this->publish_event({types::board_support_common::Event::B});
+    } else {
+        EVLOG_info << "handle_cp_state_X1: setting new duty cycle of 100.0% (ignored)";
+    }
 }
 
 void evse_board_supportImpl::handle_cp_state_F() {
     std::scoped_lock lock(this->cp_mutex);
     try {
-        EVLOG_info << "handle_cp_state_F: generating CP state F (aka EC)";
-
+        if (this->mod->controller.is_emergency()) {
+            EVLOG_info << "handle_cp_state_F: ignored, safe state already active";
+        } else {
+            if (this->cp_current_state == types::cb_board_support::CPState::B) {
+                EVLOG_info << "handle_cp_state_F: forcing safe state (via CE state B0)";
+            } else if (this->cp_current_state == types::cb_board_support::CPState::C) {
+                EVLOG_info << "handle_cp_state_F: forcing safe state (via CE state EC)";
+            } else {
+                EVLOG_info << "handle_cp_state_F: forcing safe state (in CP state A)";
+            }
+        }
+        // we can unconditionally call into the method (it already checks itself)
         this->mod->controller.set_ec_state();
     } catch (std::exception& e) {
         EVLOG_error << e.what();

modules/CbParsleyDriver/evse_board_support/evse_board_supportImpl.hpp

@@ -71,6 +71,9 @@ class evse_board_supportImpl : public evse_board_supportImplBase {
     /// @brief Tracks whether this EVSE is enabled or not.
     std::atomic_bool is_enabled {false};
 
+    /// @brief Tracks the last seen CE state.
+    std::atomic<types::cb_board_support::CEState> ce_current_state {types::cb_board_support::CEState::PowerOn};
+
     /// @brief Tracks the last published CP state.
     types::cb_board_support::CPState cp_current_state {types::cb_board_support::CPState::PowerOn};
 

modules/CbParsleyDriver/mcs/cb_mcsImpl.cpp

@@ -9,9 +9,11 @@ namespace mcs {
 void cb_mcsImpl::init() {
     // register our callback handlers
     this->mod->controller.on_id_change.connect([&](const types::cb_board_support::IDState& id_state) {
-        EVLOG_info << "ID change detected: " << this->last_id_state << " → " << id_state;
-        this->publish_id_state(id_state);
-        this->last_id_state = id_state;
+        if (this->last_id_state != id_state) {
+            EVLOG_info << "ID change detected: " << this->last_id_state << " → " << id_state;
+            this->publish_id_state(id_state);
+            this->last_id_state = id_state;
+        }
     });
 
     this->mod->controller.on_ce_change.connect([&](const types::cb_board_support::CEState& ce_state) {

modules/CbParsleyDriver/parsley/CbParsley.cpp

@@ -580,10 +580,14 @@ void CbParsley::set_ec_state() {
     n = static_cast<std::size_t>(cb_uart_com::COM_CHARGE_STATE_2);
     std::unique_lock<std::mutex> cs_lock(this->ctx_mutexes[n]);
 
-    // we should see that the safety controller enters safe state within at max 1s (FIXME)
-    if (not this->rx_cv[n].wait_for(
-            cs_lock, 1s, [&] { return cb_proto_get_safe_state_active(&this->ctx) == CS_SAFESTATE_ACTIVE_SAFESTATE; })) {
-        throw std::runtime_error("Safety Controller did not entered safe state as requested");
+    // we should see that the safety controller enters a "safe" state (but
+    // not necessarily _the_ safe state) within at max 1s (FIXME)
+    if (not this->rx_cv[n].wait_for(cs_lock, 1s, [&] {
+            return cb_proto_get_safe_state_active(&this->ctx) == CS_SAFESTATE_ACTIVE_SAFESTATE or
+                   cb_proto_get_ce_state(&this->ctx) == cs2_ce_state::CS2_CE_STATE_B0 or
+                   cb_proto_get_ce_state(&this->ctx) == cs2_ce_state::CS2_CE_STATE_EC;
+        })) {
+        throw std::runtime_error("Safety Controller did not report a safe state as expected");
     }
 }