Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Scripting: add param set and get #12889

Merged
merged 5 commits into from Feb 17, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
44 changes: 44 additions & 0 deletions libraries/AP_Param/AP_Param.cpp
Expand Up @@ -38,6 +38,9 @@ extern const AP_HAL::HAL &hal;

uint16_t AP_Param::sentinal_offset;

// singleton instance
AP_Param *AP_Param::_singleton;

#define ENABLE_DEBUG 1

#if ENABLE_DEBUG
Expand Down Expand Up @@ -379,6 +382,14 @@ bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base)
return true;
}

namespace AP {

AP_Param *param()
{
return AP_Param::get_singleton();
}

}

// find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage
Expand Down Expand Up @@ -2388,6 +2399,39 @@ bool AP_Param::set_by_name(const char *name, float value)
return false;
}

/*
get a value by name
*/
bool AP_Param::get(const char *name, float &value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this find() call is very expensive. We need to be sure that the caller does not hold any semaphores

if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
value = ((AP_Int8 *)vp)->get();
break;
case AP_PARAM_INT16:
value = ((AP_Int16 *)vp)->get();
break;

case AP_PARAM_INT32:
value = ((AP_Int32 *)vp)->get();
break;

case AP_PARAM_FLOAT:
value = ((AP_Float *)vp)->get();
break;

default:
// not a supported type
return false;
}
return true;
}

/*
set and save a value by name
*/
Expand Down
24 changes: 24 additions & 0 deletions libraries/AP_Param/AP_Param.h
Expand Up @@ -197,6 +197,11 @@ class AP_Param
uint16_t i;
for (i=0; info[i].type != AP_PARAM_NONE; i++) ;
_num_vars = i;

if (_singleton != nullptr) {
AP_HAL::panic("AP_Param must be singleton");
}
_singleton = this;
}

// empty constructor
Expand Down Expand Up @@ -281,13 +286,24 @@ class AP_Param
/// @param value The new value
/// @return true if the variable is found
static bool set_by_name(const char *name, float value);
// name helper for scripting
static bool set(const char *name, float value) { return set_by_name(name, value); };
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These helpers actually shrink the build in my test. The compiler is smart enough to just point to the same implementation, and it saves us the string space in the script code. We could just rename set_by_name in the whole code base, which I'd be fine with, but it would have been a lot of churn on this commit, and made this drag on more, so I advised this approach.


/// gat a value by name, used by scripting
///
/// @param name The full name of the variable to be found.
/// @param value A refernce to the variable
/// @return true if the variable is found
static bool get(const char *name, float &value);

/// set and save a value by name
///
/// @param name The full name of the variable to be found.
/// @param value The new value
/// @return true if the variable is found
static bool set_and_save_by_name(const char *name, float value);
// name helper for scripting
static bool set_and_save(const char *name, float value) { return set_and_save_by_name(name, value); };

/// Find a variable by index.
///
Expand Down Expand Up @@ -479,7 +495,11 @@ class AP_Param
AP_HAL::BetterStream *port);
#endif // AP_PARAM_KEY_DUMP

static AP_Param *get_singleton() { return _singleton; }

private:
static AP_Param *_singleton;

/// EEPROM header
///
/// This structure is placed at the head of the EEPROM to indicate
Expand Down Expand Up @@ -673,6 +693,10 @@ class AP_Param
void save_io_handler(void);
};

namespace AP {
AP_Param *param();
};

/// Template class for scalar variables.
///
/// Objects of this type have a value, and can be treated in many ways as though they
Expand Down
54 changes: 54 additions & 0 deletions libraries/AP_Scripting/examples/param_get_set_test.lua
@@ -0,0 +1,54 @@
-- This script is a test of param set and get
local count = 0

function update() -- this is the loop which periodically runs

-- get and print all the scripting parameters
local value = param:get('SCR_ENABLE')
if value then
gcs:send_text(6, string.format('LUA: SCR_ENABLE: %i',value))
else
gcs:send_text(6, 'LUA: get SCR_ENABLE failed')
end
value = param:get('SCR_VM_I_COUNT')
if value then
gcs:send_text(6, string.format('LUA: SCR_VM_I_COUNT: %i',value))
else
gcs:send_text(6, 'LUA: get SCR_VM_I_COUNT failed')
end
value = param:get('SCR_HEAP_SIZE')
if value then
gcs:send_text(6, string.format('LUA: SCR_HEAP_SIZE: %i',value))
else
gcs:send_text(6, 'LUA: get SCR_HEAP_SIZE failed')
end
value = param:get('SCR_DEBUG_LVL')
if value then
gcs:send_text(6, string.format('LUA: SCR_DEBUG_LVL: %i',value))
else
gcs:send_text(6, 'LUA: get SCR_DEBUG_LVL failed')
end

-- increment the script heap size by one
local heap_size = param:get('SCR_HEAP_SIZE')
if heap_size then
if not(param:set('SCR_HEAP_SIZE',heap_size + 1)) then
gcs:send_text(6, 'LUA: param set failed')
end
else
gcs:send_text(6, 'LUA: get SCR_HEAP_SIZE failed')
end


count = count + 1;

-- self terminate after 30 loops
if count > 30 then
gcs:send_text(0, 'LUA: goodbye, world')
param:set('SCR_ENABLE',0)
end

return update, 1000 -- reschedules the loop
end

return update() -- run immediately before starting to reschedule
6 changes: 6 additions & 0 deletions libraries/AP_Scripting/generator/description/bindings.desc
Expand Up @@ -183,3 +183,9 @@ singleton AP_Baro method get_external_temperature float
include AP_ESC_Telem/AP_ESC_Telem.h
singleton AP_ESC_Telem alias esc_telem
singleton AP_ESC_Telem method get_usage_seconds boolean uint8_t 0 NUM_SERVO_CHANNELS uint32_t'Null

include AP_Param/AP_Param.h
singleton AP_Param alias param
singleton AP_Param method get boolean string float'Null
singleton AP_Param method set boolean string float -FLT_MAX FLT_MAX
singleton AP_Param method set_and_save boolean string float -FLT_MAX FLT_MAX
69 changes: 69 additions & 0 deletions libraries/AP_Scripting/lua_generated_bindings.cpp
@@ -1,6 +1,7 @@
// auto generated bindings, don't manually edit. See README.md for details.
#include "lua_generated_bindings.h"
#include "lua_boxed_numerics.h"
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_SerialManager/AP_SerialManager.h>
Expand Down Expand Up @@ -531,6 +532,65 @@ const luaL_Reg Location_meta[] = {
{NULL, NULL}
};

static int AP_Param_set_and_save(lua_State *L) {
AP_Param * ud = AP_Param::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, 1, "param not supported on this firmware");
}

binding_argcheck(L, 3);
const char * data_2 = luaL_checkstring(L, 2);
const float raw_data_3 = luaL_checknumber(L, 3);
luaL_argcheck(L, ((raw_data_3 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_3 <= MIN(FLT_MAX, INFINITY))), 3, "argument out of range");
const float data_3 = raw_data_3;
const bool data = ud->set_and_save(
data_2,
data_3);

lua_pushboolean(L, data);
return 1;
}

static int AP_Param_set(lua_State *L) {
AP_Param * ud = AP_Param::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, 1, "param not supported on this firmware");
}

binding_argcheck(L, 3);
const char * data_2 = luaL_checkstring(L, 2);
const float raw_data_3 = luaL_checknumber(L, 3);
luaL_argcheck(L, ((raw_data_3 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_3 <= MIN(FLT_MAX, INFINITY))), 3, "argument out of range");
const float data_3 = raw_data_3;
const bool data = ud->set(
data_2,
data_3);

lua_pushboolean(L, data);
return 1;
}

static int AP_Param_get(lua_State *L) {
AP_Param * ud = AP_Param::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, 1, "param not supported on this firmware");
}

binding_argcheck(L, 2);
const char * data_2 = luaL_checkstring(L, 2);
float data_5003 = {};
const bool data = ud->get(
data_2,
data_5003);

if (data) {
lua_pushnumber(L, data_5003);
} else {
lua_pushnil(L);
}
return 1;
}

static int AP_ESC_Telem_get_usage_seconds(lua_State *L) {
AP_ESC_Telem * ud = AP_ESC_Telem::get_singleton();
if (ud == nullptr) {
Expand Down Expand Up @@ -1928,6 +1988,13 @@ static int AP_AHRS_get_roll(lua_State *L) {
return 1;
}

const luaL_Reg AP_Param_meta[] = {
{"set_and_save", AP_Param_set_and_save},
{"set", AP_Param_set},
{"get", AP_Param_get},
{NULL, NULL}
};

const luaL_Reg AP_ESC_Telem_meta[] = {
{"get_usage_seconds", AP_ESC_Telem_get_usage_seconds},
{NULL, NULL}
Expand Down Expand Up @@ -2185,6 +2252,7 @@ const struct userdata_meta userdata_fun[] = {
};

const struct userdata_meta singleton_fun[] = {
{"param", AP_Param_meta, NULL},
{"esc_telem", AP_ESC_Telem_meta, NULL},
{"baro", AP_Baro_meta, NULL},
{"serial", AP_SerialManager_meta, NULL},
Expand Down Expand Up @@ -2256,6 +2324,7 @@ void load_generated_bindings(lua_State *L) {
}

const char *singletons[] = {
"param",
"esc_telem",
"baro",
"serial",
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Scripting/lua_generated_bindings.h
@@ -1,5 +1,6 @@
#pragma once
// auto generated bindings, don't manually edit. See README.md for details.
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_SerialManager/AP_SerialManager.h>
Expand Down