Merge pull request #31 from LexxPluss/HWDEV-2079-improve-handling-eme…

…rgency

Hwdev 2079 improve handling emergency

lexxpluss_apps/src/actuator_controller.cpp

@@ -35,7 +35,7 @@
 #include <tuple>
 #include "actuator_controller.hpp"
 #include "adc_reader.hpp"
-#include "can_controller.hpp"
+#include "board_controller.hpp"
 #include "common.hpp"
 
 // for HW counter
@@ -632,7 +632,7 @@ class actuator_controller_impl {
         while (true) {
             for (uint32_t i{0}; i < ACTUATOR_NUM; ++i)
                 act[i].poll();
-            bool is_emergency{can_controller::is_emergency()};  // -> board controller
+            bool is_emergency{board_controller::is_emergency()};  // -> board controller
             msg_control can2actuator;
             if (k_msgq_get(&msgq_control, &can2actuator, K_NO_WAIT) == 0 && !is_emergency)
                 handle_control(can2actuator);
@@ -671,7 +671,7 @@ class actuator_controller_impl {
         pwm_trampoline_all(msg_control::DOWN, 100);
         bool stopped{wait_actuator_stop(30000, 100)};
         pwm_trampoline_all(msg_control::STOP);
-        if (!stopped || can_controller::is_emergency()) {
+        if (!stopped || board_controller::is_emergency()) {
             LOG_WRN("can not initialize location.");
             return -1;
         }
@@ -692,7 +692,7 @@ class actuator_controller_impl {
             act[i].to_location(location[i], power[i]);
         bool stopped{wait_actuator_stop(30000, 100)};
         pwm_trampoline_all(msg_control::STOP);
-        if (!stopped || can_controller::is_emergency()) {
+        if (!stopped || board_controller::is_emergency()) {
             LOG_WRN("unable to move location.");
             return -1;
         }

lexxpluss_apps/src/board_controller.cpp

@@ -39,6 +39,7 @@
 #include "can_controller.hpp"
 #include "common.hpp"
 #include "led_controller.hpp"
+#include "power_state.hpp"
 
 namespace lexxhard::board_controller {
 
@@ -1164,6 +1165,17 @@ class state_controller { // Variables Implemented
     bool is_esw_asserted(){
         return esw.is_asserted();
     }
+    bool is_emergency() const {
+        bool rtn{false};
+        if (state != POWER_STATE::OFF) {
+            rtn = esw.is_asserted() ||
+               bsw.is_asserted() ||
+               state == POWER_STATE::SUSPEND ||
+               state == POWER_STATE::RESUME_WAIT ||
+               mbd.emergency_stop_from_ros();
+        }
+        return rtn;
+    }
 private:
     static void static_poll_100ms_callback(struct k_timer *timer_id) {
         auto* instance = static_cast<state_controller*>(k_timer_user_data_get(timer_id));
@@ -1190,19 +1202,6 @@ class state_controller { // Variables Implemented
         }
     }
 
-    enum class POWER_STATE {
-        OFF,
-        WAIT_SW,
-        POST,
-        STANDBY,
-        NORMAL,
-        AUTO_CHARGE,
-        MANUAL_CHARGE,
-        LOCKDOWN,
-        TIMEROFF,
-        EMERGENCY,
-        RESUME_WAIT,
-    };
     void poll() {
         auto wheel_relay_control = [&](){
             bool wheel_poweroff{mbd.is_wheel_poweroff()};
@@ -1291,10 +1290,10 @@ class state_controller { // Variables Implemented
                 }
             } else if (ksw.is_maintenance()) {
                 LOG_DBG("maintenance mode is selected by key switch\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (esw.is_asserted()) {
                 LOG_DBG("emergency switch asserted\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (!esw.is_asserted() && !mbd.emergency_stop_from_ros() && mbd.is_ready()) {
                 LOG_DBG("not emergency and heartbeat OK\n");
                 set_new_state(POWER_STATE::NORMAL);
@@ -1320,13 +1319,13 @@ class state_controller { // Variables Implemented
                 set_new_state(POWER_STATE::STANDBY);
             } else if (ksw.is_maintenance()) {
                 LOG_DBG("maintenance mode is selected by key switch\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (esw.is_asserted()) {
                 LOG_DBG("emergency switch asserted\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.emergency_stop_from_ros()) {
                 LOG_DBG("receive emergency stop from ROS\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.is_dead()) {
                 LOG_DBG("mainboard is dead\n");
                 set_new_state(POWER_STATE::STANDBY);
@@ -1340,7 +1339,7 @@ class state_controller { // Variables Implemented
                 }
             }
             break;
-        case POWER_STATE::EMERGENCY: {
+        case POWER_STATE::SUSPEND: {
             wheel_relay_control();
             auto psw_state{psw.get_state()};
             if (!dcdc.is_ok() || psw_state == power_switch::STATE::LONG_PUSHED) {
@@ -1382,28 +1381,28 @@ class state_controller { // Variables Implemented
             wheel_relay_control();
             if (psw.get_state() != power_switch::STATE::RELEASED) {
                 LOG_DBG("detect power switch\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.power_off_from_ros()) {
                 LOG_DBG("receive power off from ROS\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (!bmu.is_ok()) {
                 LOG_DBG("BMU failure\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (!dcdc.is_ok()) {
                 LOG_DBG("DCDC failure\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (ksw.is_maintenance()) {
                 LOG_DBG("maintenance mode is selected by key switch\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (esw.is_asserted()) {
                 LOG_DBG("emergency switch asserted\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.emergency_stop_from_ros()) {
                 LOG_DBG("receive emergency stop from ROS\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.is_dead()) {
                 LOG_DBG("mainboard is dead\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (rsw.get_state() == resume_switch::STATE::PUSHED) {
                 LOG_DBG("resume switch pushed\n");
                 if (mbd.is_ready()) {
@@ -1432,13 +1431,13 @@ class state_controller { // Variables Implemented
                 set_new_state(POWER_STATE::STANDBY);
             } else if (ksw.is_maintenance()) {
                 LOG_DBG("maintenance mode is selected by key switch\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (esw.is_asserted()) {
                 LOG_DBG("emergency switch asserted\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.emergency_stop_from_ros()) {
                 LOG_DBG("receive emergency stop from ROS\n");
-                set_new_state(POWER_STATE::EMERGENCY);
+                set_new_state(POWER_STATE::SUSPEND);
             } else if (mbd.is_dead()) {
                 LOG_DBG("main board or ROS dead\n");
                 set_new_state(POWER_STATE::STANDBY);
@@ -1559,8 +1558,8 @@ class state_controller { // Variables Implemented
             k_timer_start(&charge_guard_timeout, K_MSEC(10000), K_NO_WAIT); // charge_guard_asserted = false after 10sec
             break;
 
-        case POWER_STATE::EMERGENCY:
-            LOG_INF("enter EMERGENCY\n");
+        case POWER_STATE::SUSPEND:
+            LOG_INF("enter SUSPEND\n");
             psw.set_led(true);
             wsw.set_disable(true);
             gpio_dev = GET_GPIO(v_wheel);
@@ -1626,6 +1625,7 @@ class state_controller { // Variables Implemented
         board2ros.auto_charging_status = ac.is_docked();
         board2ros.shutdown_reason = static_cast<uint32_t>(shutdown_reason);
         board2ros.wait_shutdown_state = wait_shutdown;
+        board2ros.emergency_stop = is_emergency();
         board2ros.wheel_enable = wsw.is_enabled();
 
         bool v24{false}, v_peripheral{false}, v_wheel_motor_left{false}, v_wheel_motor_right{false};
@@ -1798,6 +1798,11 @@ void run(void *p1, void *p2, void *p3)
     impl.run();
 }
 
+bool is_emergency()
+{
+    return impl.is_emergency();
+}
+
 k_thread thread;
 k_msgq msgq_board_pb_rx, msgq_board_pb_tx, msgq_can_bmu_pb;
 

lexxpluss_apps/src/board_controller.hpp

@@ -46,6 +46,7 @@ struct msg_rcv_pb {
 
 void init();
 void run(void *p1, void *p2, void *p3);
+bool is_emergency();
 extern k_thread thread;
 extern k_msgq msgq_can_bmu_pb;
 extern k_msgq msgq_board_pb_rx, msgq_board_pb_tx;

lexxpluss_apps/src/can_controller.cpp

@@ -32,6 +32,7 @@
 #include "can_controller.hpp"
 #include "board_controller.hpp"
 #include "led_controller.hpp"
+#include "power_state.hpp"
 
 namespace lexxhard::can_controller {
 
@@ -73,7 +74,7 @@ class can_controller_impl {
             }
 
             // if the board_controller state is 0 (OFF), prev_cycle_ros is reset
-            if (board2ros.state == 0) {
+            if (static_cast<POWER_STATE>(board2ros.state) == POWER_STATE::OFF) {
                 prev_cycle_ros = 0;
                 prev_cycle_send = 0;
                 ros2board.emergency_stop = true;
@@ -100,15 +101,6 @@ class can_controller_impl {
     bool get_bumper_switch() const {
 	return board2ros.bumper_switch_asserted;
     }
-    bool is_emergency() const {
-        bool rtn{false};
-        if (board2ros.state != 0) {
-            rtn = board2ros.emergency_switch_asserted ||
-               board2ros.bumper_switch_asserted ||
-               ros2board.emergency_stop;
-        }
-        return rtn;
-    }
     void brd_emgoff() {
         ros2board.emergency_stop = false;
         heartbeat_timeout = false;
@@ -211,11 +203,6 @@ bool get_bumper_switch()
     return impl.get_bumper_switch();
 }
 
-bool is_emergency()
-{
-    return impl.is_emergency();
-}
-
 k_thread thread;
 k_msgq msgq_board, msgq_control;
 

lexxpluss_apps/src/can_controller.hpp

@@ -60,6 +60,7 @@ struct msg_board {
     bool c_act_pgood, l_act_pgood, r_act_pgood;
     bool wheel_enable;
     bool charge_temperature_good;
+    bool emergency_stop;
 } __attribute__((aligned(4)));
 
 struct msg_control {
@@ -70,7 +71,6 @@ void init();
 void run(void *p1, void *p2, void *p3);
 bool get_emergency_switch();
 bool get_bumper_switch();
-bool is_emergency();
 extern k_thread thread;
 extern k_msgq /*msgq_bmu,*/ msgq_board, msgq_control;
 

lexxpluss_apps/src/led_controller.cpp

@@ -31,7 +31,7 @@
 #include <algorithm>
 #include <cstdlib>
 #include "bmu_controller.hpp"
-#include "can_controller.hpp"
+#include "board_controller.hpp"
 #include "led_controller.hpp"
 
 namespace lexxhard::led_controller {
@@ -152,7 +152,7 @@ class led_controller_impl {
     void update() {
         std::copy(&pixeldata[LED_LEFT][0],  &pixeldata[LED_LEFT][PIXELS_BACK],  &pixeldata_back[LED_LEFT][0]);
         std::copy(&pixeldata[LED_RIGHT][0], &pixeldata[LED_RIGHT][PIXELS_BACK], &pixeldata_back[LED_RIGHT][0]);
-        if (can_controller::is_emergency()) {
+        if (board_controller::is_emergency()) {
             static uint32_t blink_counter{0};
             ++blink_counter;
             if (blink_counter < 20) {

lexxpluss_apps/src/power_state.hpp

@@ -0,0 +1,13 @@
+enum class POWER_STATE {
+    OFF,
+    WAIT_SW,
+    POST,
+    STANDBY,
+    NORMAL,
+    AUTO_CHARGE,
+    MANUAL_CHARGE,
+    LOCKDOWN,
+    TIMEROFF,
+    SUSPEND,
+    RESUME_WAIT,
+};

lexxpluss_apps/src/zcan_board.hpp

@@ -81,6 +81,9 @@ class zcan_board {
             if (message.wait_shutdown_state) {
                 packedData[0] |= 0b00001000;
             }
+            if (message.emergency_stop) {
+                packedData[0] |= 0b00000100;
+            }
 
             if (message.auto_charging_status){
                 packedData[1] = 0x01;