From a0bfc823b3faf15997863ba56857165ed61fa7ce Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 11 Feb 2020 16:11:11 -0700 Subject: [PATCH] AP_Scripting: Support libraries holding the scheduler lock --- .../generator/description/bindings.desc | 1 + libraries/AP_Scripting/generator/src/main.c | 49 +++++++++++++------ .../AP_Scripting/lua_generated_bindings.cpp | 8 +++ 3 files changed, 43 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index b02c033acf..16f86bb778 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -144,6 +144,7 @@ singleton GCS method set_message_interval MAV_RESULT'enum uint8_t 0 MAVLINK_COMM include AP_Vehicle/AP_Vehicle.h singleton AP_Vehicle alias vehicle +singleton AP_Vehicle scheduler-semaphore singleton AP_Vehicle method set_mode boolean uint8_t 0 UINT8_MAX ModeReason::SCRIPTING'literal singleton AP_Vehicle method get_mode uint8_t singleton AP_Vehicle method get_likely_flying boolean diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index 8b6bd43602..faca5ca96e 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -7,20 +7,21 @@ #include #include -char keyword_alias[] = "alias"; -char keyword_ap_object[] = "ap_object"; -char keyword_comment[] = "--"; -char keyword_depends[] = "depends"; -char keyword_enum[] = "enum"; -char keyword_field[] = "field"; -char keyword_include[] = "include"; -char keyword_method[] = "method"; -char keyword_operator[] = "operator"; -char keyword_read[] = "read"; -char keyword_semaphore[] = "semaphore"; -char keyword_singleton[] = "singleton"; -char keyword_userdata[] = "userdata"; -char keyword_write[] = "write"; +char keyword_alias[] = "alias"; +char keyword_ap_object[] = "ap_object"; +char keyword_comment[] = "--"; +char keyword_depends[] = "depends"; +char keyword_enum[] = "enum"; +char keyword_field[] = "field"; +char keyword_include[] = "include"; +char keyword_method[] = "method"; +char keyword_operator[] = "operator"; +char keyword_read[] = "read"; +char keyword_scheduler_semaphore[] = "scheduler-semaphore"; +char keyword_semaphore[] = "semaphore"; +char keyword_singleton[] = "singleton"; +char keyword_userdata[] = "userdata"; +char keyword_write[] = "write"; // attributes (should include the leading ' ) char keyword_attr_enum[] = "'enum"; @@ -298,7 +299,8 @@ struct userdata_field { }; enum userdata_flags { - UD_FLAG_SEMAPHORE = (1U << 0), + UD_FLAG_SEMAPHORE = (1U << 0), + UD_FLAG_SCHEDULER_SEMAPHORE = (1U << 1), }; struct userdata_enum { @@ -788,6 +790,8 @@ void handle_singleton(void) { } else if (strcmp(type, keyword_semaphore) == 0) { node->flags |= UD_FLAG_SEMAPHORE; + } else if (strcmp(type, keyword_scheduler_semaphore) == 0) { + node->flags |= UD_FLAG_SCHEDULER_SEMAPHORE; } else if (strcmp(type, keyword_method) == 0) { handle_method(node->name, &(node->methods)); } else if (strcmp(type, keyword_enum) == 0) { @@ -845,12 +849,19 @@ void handle_ap_object(void) { } else if (strcmp(type, keyword_semaphore) == 0) { node->flags |= UD_FLAG_SEMAPHORE; + } else if (strcmp(type, keyword_scheduler_semaphore) == 0) { + node->flags |= UD_FLAG_SCHEDULER_SEMAPHORE; } else if (strcmp(type, keyword_method) == 0) { handle_method(node->name, &(node->methods)); } else { error(ERROR_SINGLETON, "AP_Objects only support aliases, methods or semaphore keyowrds (got %s)", type); } + // check that we didn't just add 2 singleton flags + if ((node->flags & UD_FLAG_SEMAPHORE) && (node->flags & UD_FLAG_SCHEDULER_SEMAPHORE)) { + error(ERROR_SINGLETON, "Taking both a library semaphore and scheduler semaphore is prohibited"); + } + // ensure no more tokens on the line if (next_token()) { error(ERROR_HEADER, "Singleton contained an unexpected extra token: %s", state.token); @@ -1345,6 +1356,10 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth fprintf(source, " ud->get_semaphore().take_blocking();\n"); } + if (data->flags & UD_FLAG_SCHEDULER_SEMAPHORE) { + fprintf(source, " AP::scheduler().get_semaphore().take_blocking();\n"); + } + switch (method->return_type.type) { case TYPE_BOOLEAN: fprintf(source, " const bool data = ud->%s(", method->name); @@ -1433,6 +1448,10 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth fprintf(source, " ud->get_semaphore().give();\n"); } + if (data->flags & UD_FLAG_SCHEDULER_SEMAPHORE) { + fprintf(source, " AP::scheduler().get_semaphore().give();\n"); + } + int return_count = 1; // number of arguments to return switch (method->return_type.type) { case TYPE_BOOLEAN: diff --git a/libraries/AP_Scripting/lua_generated_bindings.cpp b/libraries/AP_Scripting/lua_generated_bindings.cpp index 23b368b1b1..25300e9e86 100644 --- a/libraries/AP_Scripting/lua_generated_bindings.cpp +++ b/libraries/AP_Scripting/lua_generated_bindings.cpp @@ -735,8 +735,10 @@ static int AP_Vehicle_get_time_flying_ms(lua_State *L) { } binding_argcheck(L, 1); + AP::scheduler().get_semaphore().take_blocking(); const uint32_t data = ud->get_time_flying_ms(); + AP::scheduler().get_semaphore().give(); new_uint32_t(L); *static_cast(luaL_checkudata(L, -1, "uint32_t")) = data; return 1; @@ -749,8 +751,10 @@ static int AP_Vehicle_get_likely_flying(lua_State *L) { } binding_argcheck(L, 1); + AP::scheduler().get_semaphore().take_blocking(); const bool data = ud->get_likely_flying(); + AP::scheduler().get_semaphore().give(); lua_pushboolean(L, data); return 1; } @@ -762,8 +766,10 @@ static int AP_Vehicle_get_mode(lua_State *L) { } binding_argcheck(L, 1); + AP::scheduler().get_semaphore().take_blocking(); const uint8_t data = ud->get_mode(); + AP::scheduler().get_semaphore().give(); lua_pushinteger(L, data); return 1; } @@ -778,10 +784,12 @@ static int AP_Vehicle_set_mode(lua_State *L) { const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT8_MAX, UINT8_MAX))), 2, "argument out of range"); const uint8_t data_2 = static_cast(raw_data_2); + AP::scheduler().get_semaphore().take_blocking(); const bool data = ud->set_mode( data_2, ModeReason::SCRIPTING); + AP::scheduler().get_semaphore().give(); lua_pushboolean(L, data); return 1; }