diff --git a/lexxpluss_apps/src/board_controller.cpp b/lexxpluss_apps/src/board_controller.cpp index 046d6c5..7a4d971 100644 --- a/lexxpluss_apps/src/board_controller.cpp +++ b/lexxpluss_apps/src/board_controller.cpp @@ -1081,8 +1081,16 @@ class fan_driver { // Variables Implemented class boardstate_controller { // No pins declared public: void init() { + reset(); k_msgq_init(&msgq_board_pb_rx, msgq_board_pb_rx_buffer, sizeof (msg_rcv_pb), 8); } + void reset() { + heartbeat_detect = false; + ros_heartbeat_timeout = false; + emergency_stop = true; + power_off = false; + wheel_poweroff = false; + } void poll() { msg_rcv_pb msg; if (k_msgq_get(&msgq_board_pb_rx, &msg, K_NO_WAIT) == 0) { @@ -1335,7 +1343,7 @@ class state_controller { // Variables Implemented switch (state) { case POWER_STATE::OFF: - if (ksw.is_maintenance()) { + if (ksw.is_maintenance() || is_lockdown) { // empty here } else if(psw.get_state() == power_switch::STATE::RELEASED){ set_new_state(POWER_STATE::WAIT_SW); @@ -1616,13 +1624,14 @@ class state_controller { // Variables Implemented case POWER_STATE::OFF: { LOG_INF("enter OFF\n"); poweron_by_switch = false; + mbd.reset(); psw.set_led(false); rsw.set_led(false); bsw.request_reset(); dcdc.set_enable(false); // Set LED OFF - led_controller::msg const msg_led{led_controller::msg::NONE, 0}; + led_controller::msg const msg_led{led_controller::msg::NONE, 0, 1}; while (k_msgq_put(&led_controller::msgq, &msg_led, K_NO_WAIT) != 0) k_msgq_purge(&led_controller::msgq); } break; @@ -1718,6 +1727,7 @@ class state_controller { // Variables Implemented } break; case POWER_STATE::LOCKDOWN: { LOG_INF("enter LOCKDOWN\n"); + is_lockdown = true; wsw.set_disable(true); gpio_dt_spec gpio_dev = GET_GPIO(v_wheel); if (!gpio_is_ready_dt(&gpio_dev)) { @@ -1728,14 +1738,14 @@ class state_controller { // Variables Implemented ac.set_enable(false); // Set LED - led_controller::msg const msg_led{led_controller::msg::LOCKDOWN, 1000000000}; + led_controller::msg const msg_led{led_controller::msg::LOCKDOWN, 1000000000, 10}; while (k_msgq_put(&led_controller::msgq, &msg_led, K_NO_WAIT) != 0) k_msgq_purge(&led_controller::msgq); } break; case POWER_STATE::OFF_WAIT: { LOG_INF("enter OFF_WAIT\n"); timer_shutdown = k_uptime_get(); // timer reset - if (psw.get_state() == power_switch::STATE::PUSHED) + if (psw.get_state() == power_switch::STATE::PUSHED || ksw.is_maintenance()) shutdown_reason = SHUTDOWN_REASON::SWITCH; if (mbd.power_off_from_ros()) shutdown_reason = SHUTDOWN_REASON::ROS; @@ -1743,7 +1753,7 @@ class state_controller { // Variables Implemented shutdown_reason = SHUTDOWN_REASON::BMU; // Set LED - led_controller::msg const msg_led{led_controller::msg::SHOWTIME, 0}; + led_controller::msg const msg_led{led_controller::msg::SHOWTIME, 60000}; while (k_msgq_put(&led_controller::msgq, &msg_led, K_NO_WAIT) != 0) k_msgq_purge(&led_controller::msgq); } break; @@ -1832,7 +1842,7 @@ class state_controller { // Variables Implemented k_timer current_check_timeout, charge_guard_timeout; const device *dev_wdi{nullptr}; bool poweron_by_switch{false}, current_check_enable{false}, charge_guard_asserted{false}, - last_wheel_poweroff{false}; + last_wheel_poweroff{false}, is_lockdown{false}; } impl; int cmd_power_on(const shell *shell, size_t argc, char **argv) diff --git a/lexxpluss_apps/src/led_controller.cpp b/lexxpluss_apps/src/led_controller.cpp index 75c7c3a..4ef9090 100644 --- a/lexxpluss_apps/src/led_controller.cpp +++ b/lexxpluss_apps/src/led_controller.cpp @@ -49,14 +49,20 @@ class led_message_receiver { bool get_message(msg &output) { bool updated{false}; if (msg message_new; k_msgq_get(&msgq, &message_new, K_MSEC(DELAY_MS)) == 0) { - if (message.interrupt_ms > 0) { + if (message.priority < message_new.priority) { + LOG_INF("interrupted pattern %u %ums %dpri", message_new.pattern, message_new.interrupt_ms, message_new.priority); + message_interrupted = message; + cycle_interrupted = k_cycle_get_32(); + message = message_new; + updated = true; + } else if (message.interrupt_ms > 0) { if (message_new.interrupt_ms == 0) { message_interrupted = message_new; } } else { if (is_new_message(message_new)) { if (message_new.interrupt_ms > 0) { - LOG_INF("interrupted pattern %u %ums", message_new.pattern, message_new.interrupt_ms); + LOG_INF("interrupted pattern %u %ums %dpri", message_new.pattern, message_new.interrupt_ms, message_new.priority); message_interrupted = message; cycle_interrupted = k_cycle_get_32(); } diff --git a/lexxpluss_apps/src/led_controller.hpp b/lexxpluss_apps/src/led_controller.hpp index 89b3683..85908da 100644 --- a/lexxpluss_apps/src/led_controller.hpp +++ b/lexxpluss_apps/src/led_controller.hpp @@ -39,8 +39,8 @@ struct can_format { struct msg { msg() : pattern(NONE), interrupt_ms(0) {} - msg(uint32_t pattern, uint32_t interrupt_ms) : - pattern(pattern), interrupt_ms(interrupt_ms) {} + msg(uint32_t pattern, uint32_t interrupt_ms, uint8_t priority=0) : + pattern(pattern), interrupt_ms(interrupt_ms), priority(priority) {} msg(can_format frame) : pattern(frame.pattern), cpm(frame.count_per_minutes), rgb{frame.rgb[0],frame.rgb[1],frame.rgb[2]} {} msg(const char *str) { @@ -104,6 +104,7 @@ struct msg { uint8_t pattern{NONE}; uint32_t interrupt_ms{0}, cpm{0}; uint8_t rgb[3]{0, 0, 0}; + uint8_t priority{0}; static constexpr uint8_t NONE{0}; static constexpr uint8_t EMERGENCY_STOP{1}; static constexpr uint8_t AMR_MODE{2};