From 7b5e157842fab6c67e0d103a45765832f4214bb6 Mon Sep 17 00:00:00 2001 From: Gerson Fernando Budke Date: Thu, 8 Aug 2024 21:12:09 +0200 Subject: [PATCH] Add sleep action Signed-off-by: Gerson Fernando Budke --- include/zephyr/zephyrbt/zephyrbt.h | 7 +++ ...enerate-zephyrbt-from-behaviourtreecpp-xml | 3 + subsys/zephyrbt/CMakeLists.txt | 1 + subsys/zephyrbt/zephyrbt_action_sleep.c | 63 +++++++++++++++++++ 4 files changed, 74 insertions(+) create mode 100644 subsys/zephyrbt/zephyrbt_action_sleep.c diff --git a/include/zephyr/zephyrbt/zephyrbt.h b/include/zephyr/zephyrbt/zephyrbt.h index e26ec2d..a1ae5a6 100644 --- a/include/zephyr/zephyrbt/zephyrbt.h +++ b/include/zephyr/zephyrbt/zephyrbt.h @@ -131,6 +131,13 @@ enum zephyrbt_child_status zephyrbt_action_always_success(struct zephyrbt_contex struct zephyrbt_node *self); enum zephyrbt_child_status zephyrbt_action_always_failure(struct zephyrbt_context *ctx, struct zephyrbt_node *self); +enum zephyrbt_sleep_attributes { + ZEPHYRBT_SLEEP_ATTRIBUTE_MSEC, +}; +enum zephyrbt_child_status zephyrbt_action_sleep(struct zephyrbt_context *ctx, + struct zephyrbt_node *self); +enum zephyrbt_child_status zephyrbt_action_sleep_init(struct zephyrbt_context *ctx, + struct zephyrbt_node *self); enum zephyrbt_child_status zephyrbt_control_fallback(struct zephyrbt_context *ctx, struct zephyrbt_node *self); enum zephyrbt_child_status zephyrbt_control_sequence(struct zephyrbt_context *ctx, diff --git a/scripts/generate-zephyrbt-from-behaviourtreecpp-xml b/scripts/generate-zephyrbt-from-behaviourtreecpp-xml index 08c482c..4e156a6 100755 --- a/scripts/generate-zephyrbt-from-behaviourtreecpp-xml +++ b/scripts/generate-zephyrbt-from-behaviourtreecpp-xml @@ -428,6 +428,8 @@ def main(zephyrbt_filename, output_inc, output_data, output_stub, stack, prio, user_file) -> None: zephyrbt = Path(zephyrbt_filename).stem + sleep = [['msec', 'input_port', None, 'Wait the amount of milliseconds']] + parallel = [['failure_count', 'input_port', '1', 'number of children which need to fail to trigger a FAILURE'], ['success_count', 'input_port', '-1', 'number of children which need to succeed to trigger a SUCCESS'] ] @@ -439,6 +441,7 @@ def main(zephyrbt_filename, output_inc, output_data, output_stub, # node_name, type, has_init, blackboard_attributes builtin = [('always_success', 'action', False, []), ('always_failure', 'action', False, []), + ('sleep', 'action', True, sleep), ('fallback', 'control', False, []), ('sequence', 'control', False, []), ('parallel', 'control', True, parallel), diff --git a/subsys/zephyrbt/CMakeLists.txt b/subsys/zephyrbt/CMakeLists.txt index 92afc5c..67edcd0 100644 --- a/subsys/zephyrbt/CMakeLists.txt +++ b/subsys/zephyrbt/CMakeLists.txt @@ -10,6 +10,7 @@ zephyr_sources_ifdef(CONFIG_ZEPHYR_BEHAVIOUR_TREE ) zephyr_sources_ifdef(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_CONTEXT + zephyrbt_action_sleep.c zephyrbt_decorator_delay.c zephyrbt_decorator_run_once.c zephyrbt_decorator_timeout.c diff --git a/subsys/zephyrbt/zephyrbt_action_sleep.c b/subsys/zephyrbt/zephyrbt_action_sleep.c new file mode 100644 index 0000000..78ec248 --- /dev/null +++ b/subsys/zephyrbt/zephyrbt_action_sleep.c @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2024 O.S. Systems Software LTDA. + * Copyright (c) 2024 Freedom Veiculos Eletricos + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +LOG_MODULE_REGISTER(zephyrbt_action_sleep, CONFIG_ZEPHYR_BEHAVIOUR_TREE_LOG_LEVEL); + +struct zephyrbt_action_sleep_context { + struct zephyrbt_blackboard_item *msec; +}; + +enum zephyrbt_child_status zephyrbt_action_sleep_init(struct zephyrbt_context *ctx, + struct zephyrbt_node *self) +{ +#if defined(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO) + LOG_DBG("init: %s", self->name); +#endif + + struct zephyrbt_action_sleep_context *sleep; + + sleep = k_malloc(sizeof(struct zephyrbt_action_sleep_context)); + self->ctx = sleep; + + if (sleep == NULL) { + LOG_ERR("Context can not be allocate."); + return ZEPHYRBT_CHILD_FAILURE_STATUS; + } + + memset(sleep, 0, sizeof(struct zephyrbt_action_sleep_context)); + + sleep->msec = zephyrbt_search_blackboard(ctx, self->index, ZEPHYRBT_SLEEP_ATTRIBUTE_MSEC); + + if (sleep->msec == NULL) { + LOG_ERR("Invalid sleep msec value."); + return ZEPHYRBT_CHILD_FAILURE_STATUS; + } + + return ZEPHYRBT_CHILD_SUCCESS_STATUS; +} + +enum zephyrbt_child_status zephyrbt_action_sleep(struct zephyrbt_context *ctx, + struct zephyrbt_node *self) +{ +#if defined(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO) + LOG_DBG("%s", self->name); +#endif + + struct zephyrbt_action_sleep_context *sleep; + sleep = (struct zephyrbt_action_sleep_context *)self->ctx; + + if (sleep == NULL) { + LOG_ERR("Undefined behaviour on zephyrbt_action_sleep."); + return ZEPHYRBT_CHILD_FAILURE_STATUS; + } + + k_msleep((uintptr_t)sleep->msec->item); + + return ZEPHYRBT_CHILD_SUCCESS_STATUS; +}