diff --git a/CMakeLists.txt b/CMakeLists.txt index 0727ec4..fcd79d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,7 @@ include_directories( ${UBX_INCLUDE_DIR} ${BRICS_3D_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIR} + $ENV{UBX_ROOT}/std_blocks/ptrig ) LINK_DIRECTORIES(${BRICS_3D_LINK_DIRECTORIES}) @@ -87,3 +88,22 @@ install(TARGETS fmpcmockuplib DESTINATION ${INSTALL_LIB_BLOCKS_DIR} EXPORT fmpcm set_property(TARGET fmpcmockuplib PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE) install(EXPORT fmpcmockuplib-block DESTINATION ${INSTALL_CMAKE_DIR}) +# Compile library luacoordinatorlib +add_library(luacoordinatorlib SHARED src/lua_coordinator.c) +set_target_properties(luacoordinatorlib PROPERTIES PREFIX "") +target_link_libraries(luacoordinatorlib ${UBX_LIBRARIES}) + +# Install luacoordinatorlib +install(TARGETS luacoordinatorlib DESTINATION ${INSTALL_LIB_BLOCKS_DIR} EXPORT luacoordinatorlib-block) +set_property(TARGET luacoordinatorlib PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE) +install(EXPORT luacoordinatorlib-block DESTINATION ${INSTALL_CMAKE_DIR}) + +# Compile library motioncoordinatorlib +add_library(motioncoordinatorlib SHARED src/motion_coordinator.c) +set_target_properties(motioncoordinatorlib PROPERTIES PREFIX "") +target_link_libraries(motioncoordinatorlib ${UBX_LIBRARIES}) + +# Install motioncoordinatorlib +install(TARGETS motioncoordinatorlib DESTINATION ${INSTALL_LIB_BLOCKS_DIR} EXPORT motioncoordinatorlib-block) +set_property(TARGET motioncoordinatorlib PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE) +install(EXPORT motioncoordinatorlib-block DESTINATION ${INSTALL_CMAKE_DIR}) diff --git a/src/lua_coordinator.c b/src/lua_coordinator.c new file mode 100644 index 0000000..8733587 --- /dev/null +++ b/src/lua_coordinator.c @@ -0,0 +1,257 @@ +/* + * A generic luajit based block. + */ + +#define DEBUG + +#include +#include +#include + +#include +#include + +#include "ubx.h" + +static struct ubx_node_info *global_ni; + +//#define LUA_BLOCK_FILE "/home/mk/prog/c/microblx/std_blocks/lua_coordinator/lua_coordinator.lua" +#define EXEC_STR_BUFF_SIZE 16*1024*1024 + +ubx_port_t lua_ports[] = { + { .name="exec_str", .in_type_name="char", .out_type_name="int", .in_data_len=16777216 }, + { NULL } +}; + +def_write_fun(write_int, int) + +ubx_config_t lua_conf[] = { + { .name="lua_file", .type_name="char", .value = { .len=32 } }, + { .name="lua_str", .type_name="char", .value = { .len=32 } }, + { .name="trig_blocks", .type_name = "struct ptrig_config", .doc = "List of blocks that are coordinated by this block." }, + { NULL } +}; + +char lua_coordinator_meta[] = + "{ doc='A generic luajit based block'," + " license='MIT'," + " real-time=false," + "}"; + +struct lua_coordinator_info { + struct ubx_node_info* ni; + struct lua_State* L; + ubx_data_t* exec_str_buff; +}; + + + +/** + * @brief: call a hook with fname. + * + * @param block (is passed on a first arg) + * @param fname name of function to call + * @param require_fun raise an error if function fname does not exist. + * @param require_res if 1, require a boolean valued result. + * @return -1 in case of error, 0 otherwise. + */ +int call_hook(ubx_block_t* b, const char *fname, int require_fun, int require_res) +{ + int ret = 0; + struct lua_coordinator_info* inf = (struct lua_coordinator_info*) b->private_data; + int num_res = (require_res != 0) ? 1 : 0; + + lua_getglobal(inf->L, fname); + + if(lua_isnil(inf->L, -1)) { + lua_pop(inf->L, 1); + if(require_fun) + ERR("%s: no (required) Lua function %s", b->name, fname); + else + goto out; + } + + lua_pushlightuserdata(inf->L, (void*) b); + + if (lua_pcall(inf->L, 1, num_res, 0) != 0) { + ERR("%s: error calling function %s: %s", b->name, fname, lua_tostring(inf->L, -1)); + ret = -1; + goto out; + } + + if(require_res) { + if (!lua_isboolean(inf->L, -1)) { + ERR("%s: %s must return a bool but returned a %s", + b->name, fname, lua_typename(inf->L, lua_type(inf->L, -1))); + ret = -1; + goto out; + } + ret = !(lua_toboolean(inf->L, -1)); /* back in C! */ + lua_pop(inf->L, 1); /* pop result */ + } + out: + return ret; +} + +/** + * init_lua_state - initalize lua_State and execute lua_file. + * + * @param inf + * @param lua_file + * + * @return 0 if Ok, -1 otherwise. + */ +static int init_lua_state(struct lua_coordinator_info* inf, const char* lua_file, const char* lua_str) +{ + int ret=-1; + + if((inf->L=luaL_newstate())==NULL) { + ERR("failed to alloc lua_State"); + goto out; + } + + luaL_openlibs(inf->L); + + if(lua_file) { + if((ret=luaL_dofile(inf->L, lua_file))!=0) { + ERR("Failed to load lua_file '%s': %s\n", lua_file, lua_tostring(inf->L, -1)); + goto out; + } + } + + if(lua_str) { + if((ret=luaL_dostring(inf->L, lua_str))!=0) { + ERR("Failed to load lua_str '%s': %s\n", lua_str, lua_tostring(inf->L, -1)); + goto out; + } + } + + ret=0; + out: + return ret; +} + + +static int lua_coordinator_init(ubx_block_t *b) +{ + char *lua_file, *lua_str; + unsigned int lua_file_len, lua_str_len; + + int ret = -EOUTOFMEM; + struct lua_coordinator_info* inf; + + if((inf = calloc(1, sizeof(struct lua_coordinator_info)))==NULL) + goto out; + + b->private_data = inf; + + inf->ni=global_ni; + + lua_file = (char *) ubx_config_get_data_ptr(b, "lua_file", &lua_file_len); + lua_file = (!strncmp(lua_file, "", lua_file_len)) ? NULL : lua_file; + + lua_str = (char *) ubx_config_get_data_ptr(b, "lua_str", &lua_str_len); + lua_str = (!strncmp(lua_str, "", lua_str_len)) ? NULL : lua_str; + + if((inf->exec_str_buff = ubx_data_alloc(b->ni, "char", EXEC_STR_BUFF_SIZE)) == NULL) { + ERR("failed to allocate exec_str buffer"); + goto out_free1; + } + + if(init_lua_state(inf, lua_file, lua_str) != 0) + goto out_free2; + + if((ret=call_hook(b, "init", 0, 1)) != 0) + goto out_free2; + + /* Ok! */ + ret = 0; + goto out; + + out_free2: + free(inf->exec_str_buff); + out_free1: + free(inf); + out: + return ret; +} + +static int lua_coordinator_start(ubx_block_t *b) +{ + return call_hook(b, "start", 0, 1); +} + +/** + * lua_coordinator_step - execute lua string and call step hook + * + * @param b + */ +static void lua_coordinator_step(ubx_block_t *b) +{ + int len=0, ret; + struct lua_coordinator_info* inf = (struct lua_coordinator_info*) b->private_data; + + /* any lua code to execute */ + ubx_port_t* p_exec_str = ubx_port_get(b, "exec_str"); + + if((len=__port_read(p_exec_str, inf->exec_str_buff))>0) { + if ((ret=luaL_dostring(inf->L, inf->exec_str_buff->data)) != 0) { + ERR("Failed to exec_str: %s", lua_tostring(inf->L, -1)); + goto out; + } + } + call_hook(b, "step", 0, 0); + out: + /* TODO: fix this. realloc could have changed port addr */ + if(len>0) { + p_exec_str = ubx_port_get(b, "exec_str"); + write_int(p_exec_str, &ret); + } + return; +} + +static void lua_coordinator_stop(ubx_block_t *b) +{ + call_hook(b, "stop", 0, 0); +} + +static void lua_coordinator_cleanup(ubx_block_t *b) +{ + struct lua_coordinator_info* inf = (struct lua_coordinator_info*) b->private_data; + call_hook(b, "cleanup", 0, 0); + lua_close(inf->L); + ubx_data_free(b->ni, inf->exec_str_buff); + free(b->private_data); +} + + +/* put everything together */ +ubx_block_t lua_comp = { + .name = "lua/lua_coordinator", + .type = BLOCK_TYPE_COMPUTATION, + .meta_data = lua_coordinator_meta, + .configs = lua_conf, + .ports = lua_ports, + + /* ops */ + .init = lua_coordinator_init, + .start = lua_coordinator_start, + .step = lua_coordinator_step, + .stop = lua_coordinator_stop, + .cleanup = lua_coordinator_cleanup, +}; + +static int lua_init(ubx_node_info_t* ni) +{ + global_ni=ni; + return ubx_block_register(ni, &lua_comp); +} + +static void lua_cleanup(ubx_node_info_t *ni) +{ + ubx_block_unregister(ni, "lua/lua_coordinator"); +} + +UBX_MODULE_INIT(lua_init) +UBX_MODULE_CLEANUP(lua_cleanup) +UBX_MODULE_LICENSE_SPDX(BSD-3-Clause) diff --git a/src/motion_coordinator.c b/src/motion_coordinator.c new file mode 100644 index 0000000..6321c85 --- /dev/null +++ b/src/motion_coordinator.c @@ -0,0 +1,152 @@ + +#define DEBUG + +#include +#include +#include "ubx.h" +#include "types/ptrig_config.h" // (still) part of std_blocks in microblox +#include "types/ptrig_config.h.hexarr" + +ubx_port_t motion_coordinator_ports[] = { + { .name="cmd_in", .in_type_name="unsigned char", .in_data_len=1, .doc="Input data port that toggles the controller on/off." }, + { NULL } +}; + +ubx_config_t motion_coordinator_conf[] = { + { .name="trig_blocks", .type_name = "struct ptrig_config", .doc = "List of blocks that are coordinated by this block." }, + { NULL } +}; + +char motion_coordinator_meta[] = + "{ doc='A motion coordination block'," + " license='BSD-3-Clause'," + " real-time=false," + "}"; + +struct motion_coordinator_info { + struct ptrig_config* trig_list; /* similar as in ptrig block */ + unsigned int trig_list_len; +}; + +def_read_fun(read_command, unsigned char) + + +static int motion_coordinator_init(ubx_block_t *b) +{ + int ret=0; + + DBG(" "); + if ((b->private_data = calloc(1, sizeof(struct motion_coordinator_info)))==NULL) { + ERR("Failed to alloc memory"); + ret=EOUTOFMEM; + goto out; + } + + + struct motion_coordinator_info *inf = b->private_data; + + /* Get list of blocks that are coordinated */ + ubx_data_t* trig_list_data = 0; + trig_list_data = ubx_config_get_data(b, "trig_blocks"); + if (trig_list_data != 0) { + inf->trig_list = (struct ptrig_config*)trig_list_data->data; + inf->trig_list_len = 2u; //(unsigned int)trig_list_data->len; + printf("motion_coordinator: has %i blocks thet are beeind coordinated.", inf->trig_list_len); + } else { + ret=-1; + goto out; + } + + + out: + return ret; +} + +static int motion_coordinator_start(ubx_block_t *b) +{ + printf("motion_coordinator_start\n"); + return 0; +} + + +static void motion_coordinator_step(ubx_block_t *b) +{ + printf("motion_coordinator_step\n"); + unsigned char cmd; // A complete "command message" + int32_t readBytes = 0; + int idx=0; + + struct motion_coordinator_info* inf = (struct motion_coordinator_info*)(b->private_data); + + ubx_port_t* cmd_port = ubx_port_get(b, "cmd_in"); + readBytes = read_command(cmd_port, &cmd); + + if (readBytes == 1) { + switch (cmd) { + case 0x00: + + printf("motion_coordinator: OFF\n"); + for(idx=0; idxtrig_list_len; idx++) { + ubx_block_stop(inf->trig_list[idx].b); + } + break; + + case 0x01: + + printf("motion_coordinator: ON\n"); + for(idx=0; idxtrig_list_len; idx++) { + ubx_block_start(inf->trig_list[idx].b); + } + break; + + default: + + printf("motion_coordinator: unknown commnad"); + break; + } + } + + + +} + +static void motion_coordinator_stop(ubx_block_t *b) +{ + +} + +static void motion_coordinator_cleanup(ubx_block_t *b) +{ + free(b->private_data); +} + + +/* put everything together */ +ubx_block_t motion_coord = { + .name = "motion_coordinator", + .type = BLOCK_TYPE_COMPUTATION, + .meta_data = motion_coordinator_meta, + .configs = motion_coordinator_conf, + .ports = motion_coordinator_ports, + + /* ops */ + .init = motion_coordinator_init, + .start = motion_coordinator_start, + .step = motion_coordinator_step, + .stop = motion_coordinator_stop, + .cleanup = motion_coordinator_cleanup, +}; + +static int motion_coodinatior_mod_init(ubx_node_info_t* ni) +{ + return ubx_block_register(ni, &motion_coord); +} + +static void motion_coodinatior_mod_a_cleanup(ubx_node_info_t *ni) +{ + ubx_block_unregister(ni, "motion_coordinator"); +} + +UBX_MODULE_INIT(motion_coodinatior_mod_init) +UBX_MODULE_CLEANUP(motion_coodinatior_mod_a_cleanup) +UBX_MODULE_LICENSE_SPDX(BSD-3-Clause) diff --git a/src/motion_coordinator.lua b/src/motion_coordinator.lua new file mode 100644 index 0000000..6ce0fc0 --- /dev/null +++ b/src/motion_coordinator.lua @@ -0,0 +1,82 @@ + +local ubx=require "ubx" +local ffi = require("ffi") + + +local coordinated_block + +function init(block) + print("init") + + --ubx.config_add(block, "trig_blocks", "Blocks that will be toggeled on/off.", "struct ptrig_config", 1) -- this one does NOT work + ubx.config_add(block, "test", "Just a test", "int", 1) + ubx.port_add(block, "motion_toggler", "Input data port that toggles the controller on/off.", "unsigned char", 1, nil, 0, 0) + + return true +end + +function start(block) + print("motion_coordinator: start") + print(block) + local r = ubx.resolve_types(block) + + local ptab_before = ubx.ports_map(block, ubx.port_totab) + + -- PROBLEM it is not possible to read the below config file :-(! + + local bt=ubx.block_totab(block) + -- results in: ERR call_hook: motion_coordinator: error calling function start: ...microblx/lua/ubx.lua:318: attempt to index local 'b' (a userdata value) + + local config = ubx.config_get_data(block, "trig_blocks") -- cannot be passed as .usc configuration parameter? + --local trig_blocks = ubx.data_tolua(ubx.config_get_data(block, "trig_blocks")) + + local cb = ubx.data_tolua(config) + -- ERR call_hook: motion_coordinator: error calling function start: .../microblx/lua/ubx.lua:497: undeclared or implicit tag 'ptrig_config' + + print(cb) + + coordinated_block = cb.b + + + local test = ubx.config_get(block, "test") + print(ubx.config_tostr(test)) + + return true +end + +function step(block) + + -- read data at port + local res, data = ubx.port_read(ubx.port_get(block, "motion_toggler")) + print("motion_coordinator: res = " .. res .. " data = " .. ubx.data_tostr(data)); + + + if res > 0 then + print ("\tNew toggle command. Setting to: " .. ubx.data_tolua(data)) + + -- handle the "protocol" + if ubx.data_tolua(data) == 0 then + print("\t Motion = OFF") + ubx.block_stop(coordinated_block) + elseif ubx.data_tolua(data) == 1 then + print("\t Motion = ON") + ubx.block_start(coordinated_block) + else + print("Unknown command.") + end + + end + +end + +function stop(block) + print("motion_coordinator: stop") + +end + +function cleanup(block) + print("motion_coordinator: cleanup") + --print("rm'ing", ubx.port_rm(block, "motion_toggler")) + --print("rm'ing", ubx.config_rm(block, "trig_blocks")) + print("rm'ing", ubx.config_rm(block, "test")) +end