mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
AP_Scripting: Manual bindings: allow both : and . acsess
This commit is contained in:
parent
2c8c037b93
commit
5b2236e88c
@ -66,15 +66,18 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
return luaL_argerror(L, 1, "logger not supported on this firmware");
|
return luaL_argerror(L, 1, "logger not supported on this firmware");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Allow : and . access
|
||||||
|
const int arg_offset = (luaL_testudata(L, 1, "logger") != NULL) ? 1 : 0;
|
||||||
|
|
||||||
// check we have at least 4 arguments passed in
|
// check we have at least 4 arguments passed in
|
||||||
const int args = lua_gettop(L);
|
const int args = lua_gettop(L) - arg_offset;
|
||||||
if (args < 4) {
|
if (args < 4) {
|
||||||
return luaL_argerror(L, args, "too few arguments");
|
return luaL_argerror(L, args, "too few arguments");
|
||||||
}
|
}
|
||||||
|
|
||||||
const char * name = luaL_checkstring(L, 1);
|
const char * name = luaL_checkstring(L, 1 + arg_offset);
|
||||||
const char * labels = luaL_checkstring(L, 2);
|
const char * labels = luaL_checkstring(L, 2 + arg_offset);
|
||||||
const char * fmt = luaL_checkstring(L, 3);
|
const char * fmt = luaL_checkstring(L, 3 + arg_offset);
|
||||||
|
|
||||||
// cheack the name, labels and format are not too long
|
// cheack the name, labels and format are not too long
|
||||||
if (strlen(name) >= LS_NAME_SIZE) {
|
if (strlen(name) >= LS_NAME_SIZE) {
|
||||||
@ -132,8 +135,8 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
} else {
|
} else {
|
||||||
// read in units and multiplers strings
|
// read in units and multiplers strings
|
||||||
field_start += 2;
|
field_start += 2;
|
||||||
const char * units = luaL_checkstring(L, 4);
|
const char * units = luaL_checkstring(L, 4 + arg_offset);
|
||||||
const char * multipliers = luaL_checkstring(L, 5);
|
const char * multipliers = luaL_checkstring(L, 5 + arg_offset);
|
||||||
|
|
||||||
if (length != strlen(units)) {
|
if (length != strlen(units)) {
|
||||||
return luaL_error(L, "units must be same length as format");
|
return luaL_error(L, "units must be same length as format");
|
||||||
@ -180,6 +183,7 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
for (uint8_t i=field_start; i<=args; i++) {
|
for (uint8_t i=field_start; i<=args; i++) {
|
||||||
uint8_t charlen = 0;
|
uint8_t charlen = 0;
|
||||||
uint8_t index = have_units ? i-5 : i-3;
|
uint8_t index = have_units ? i-5 : i-3;
|
||||||
|
uint8_t arg_index = i + arg_offset;
|
||||||
switch(fmt_cat[index]) {
|
switch(fmt_cat[index]) {
|
||||||
// logger varable types not available to scripting
|
// logger varable types not available to scripting
|
||||||
// 'b': int8_t
|
// 'b': int8_t
|
||||||
@ -194,15 +198,15 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
case 'i':
|
case 'i':
|
||||||
case 'L':
|
case 'L':
|
||||||
case 'e': {
|
case 'e': {
|
||||||
const lua_Integer tmp1 = luaL_checkinteger(L, i);
|
const lua_Integer tmp1 = luaL_checkinteger(L, arg_index);
|
||||||
luaL_argcheck(L, ((tmp1 >= INT32_MIN) && (tmp1 <= INT32_MAX)), i, "argument out of range");
|
luaL_argcheck(L, ((tmp1 >= INT32_MIN) && (tmp1 <= INT32_MAX)), arg_index, "argument out of range");
|
||||||
int32_t tmp = tmp1;
|
int32_t tmp = tmp1;
|
||||||
luaL_addlstring(&buffer, (char *)&tmp, sizeof(int32_t));
|
luaL_addlstring(&buffer, (char *)&tmp, sizeof(int32_t));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case 'f': {
|
case 'f': {
|
||||||
float tmp = luaL_checknumber(L, i);
|
float tmp = luaL_checknumber(L, arg_index);
|
||||||
luaL_argcheck(L, ((tmp >= -INFINITY) && (tmp <= INFINITY)), i, "argument out of range");
|
luaL_argcheck(L, ((tmp >= -INFINITY) && (tmp <= INFINITY)), arg_index, "argument out of range");
|
||||||
luaL_addlstring(&buffer, (char *)&tmp, sizeof(float));
|
luaL_addlstring(&buffer, (char *)&tmp, sizeof(float));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -212,15 +216,15 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
}
|
}
|
||||||
case 'M':
|
case 'M':
|
||||||
case 'B': {
|
case 'B': {
|
||||||
const lua_Integer tmp1 = luaL_checkinteger(L, i);
|
const lua_Integer tmp1 = luaL_checkinteger(L, arg_index);
|
||||||
luaL_argcheck(L, ((tmp1 >= 0) && (tmp1 <= UINT8_MAX)), i, "argument out of range");
|
luaL_argcheck(L, ((tmp1 >= 0) && (tmp1 <= UINT8_MAX)), arg_index, "argument out of range");
|
||||||
uint8_t tmp = static_cast<uint8_t>(tmp1);
|
uint8_t tmp = static_cast<uint8_t>(tmp1);
|
||||||
luaL_addlstring(&buffer, (char *)&tmp, sizeof(uint8_t));
|
luaL_addlstring(&buffer, (char *)&tmp, sizeof(uint8_t));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case 'I':
|
case 'I':
|
||||||
case 'E': {
|
case 'E': {
|
||||||
const uint32_t tmp = coerce_to_uint32_t(L, i);
|
const uint32_t tmp = coerce_to_uint32_t(L, arg_index);
|
||||||
luaL_addlstring(&buffer, (char *)&tmp, sizeof(uint32_t));
|
luaL_addlstring(&buffer, (char *)&tmp, sizeof(uint32_t));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -233,14 +237,14 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
default: {
|
default: {
|
||||||
return luaL_error(L, "%c unsupported format",fmt_cat[i-3]);
|
return luaL_error(L, "%c unsupported format",fmt_cat[arg_index-3]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (charlen != 0) {
|
if (charlen != 0) {
|
||||||
const char *tmp = luaL_checkstring(L, i);
|
const char *tmp = luaL_checkstring(L, arg_index);
|
||||||
const size_t slen = strlen(tmp);
|
const size_t slen = strlen(tmp);
|
||||||
if (slen > charlen) {
|
if (slen > charlen) {
|
||||||
return luaL_error(L, "arg %i too long for %c format",i,fmt_cat[i-3]);
|
return luaL_error(L, "arg %i too long for %c format",arg_index,fmt_cat[arg_index-3]);
|
||||||
}
|
}
|
||||||
char tstr[charlen];
|
char tstr[charlen];
|
||||||
memcpy(tstr, tmp, slen);
|
memcpy(tstr, tmp, slen);
|
||||||
@ -261,7 +265,10 @@ int AP_Logger_Write(lua_State *L) {
|
|||||||
|
|
||||||
int lua_get_i2c_device(lua_State *L) {
|
int lua_get_i2c_device(lua_State *L) {
|
||||||
|
|
||||||
const int args = lua_gettop(L);
|
// Allow : and . access
|
||||||
|
const int arg_offset = (luaL_testudata(L, 1, "i2c") != NULL) ? 1 : 0;
|
||||||
|
|
||||||
|
const int args = lua_gettop(L) - arg_offset;
|
||||||
if (args < 2) {
|
if (args < 2) {
|
||||||
return luaL_argerror(L, args, "require i2c bus and address");
|
return luaL_argerror(L, args, "require i2c bus and address");
|
||||||
}
|
}
|
||||||
@ -269,12 +276,12 @@ int lua_get_i2c_device(lua_State *L) {
|
|||||||
return luaL_argerror(L, args, "too many arguments");
|
return luaL_argerror(L, args, "too many arguments");
|
||||||
}
|
}
|
||||||
|
|
||||||
const lua_Integer bus_in = luaL_checkinteger(L, 1);
|
const lua_Integer bus_in = luaL_checkinteger(L, 1 + arg_offset);
|
||||||
luaL_argcheck(L, ((bus_in >= 0) && (bus_in <= 4)), 1, "bus out of range");
|
luaL_argcheck(L, ((bus_in >= 0) && (bus_in <= 4)), 1 + arg_offset, "bus out of range");
|
||||||
const uint8_t bus = static_cast<uint8_t>(bus_in);
|
const uint8_t bus = static_cast<uint8_t>(bus_in);
|
||||||
|
|
||||||
const lua_Integer address_in = luaL_checkinteger(L, 2);
|
const lua_Integer address_in = luaL_checkinteger(L, 2 + arg_offset);
|
||||||
luaL_argcheck(L, ((address_in >= 0) && (address_in <= 128)), 2, "address out of range");
|
luaL_argcheck(L, ((address_in >= 0) && (address_in <= 128)), 2 + arg_offset, "address out of range");
|
||||||
const uint8_t address = static_cast<uint8_t>(address_in);
|
const uint8_t address = static_cast<uint8_t>(address_in);
|
||||||
|
|
||||||
// optional arguments, use the same defaults as the hal get_device function
|
// optional arguments, use the same defaults as the hal get_device function
|
||||||
@ -282,10 +289,10 @@ int lua_get_i2c_device(lua_State *L) {
|
|||||||
bool use_smbus = false;
|
bool use_smbus = false;
|
||||||
|
|
||||||
if (args > 2) {
|
if (args > 2) {
|
||||||
bus_clock = coerce_to_uint32_t(L, 3);
|
bus_clock = coerce_to_uint32_t(L, 3 + arg_offset);
|
||||||
|
|
||||||
if (args > 3) {
|
if (args > 3) {
|
||||||
use_smbus = static_cast<bool>(lua_toboolean(L, 4));
|
use_smbus = static_cast<bool>(lua_toboolean(L, 4 + arg_offset));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -316,10 +323,13 @@ int lua_get_i2c_device(lua_State *L) {
|
|||||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
||||||
int lua_get_CAN_device(lua_State *L) {
|
int lua_get_CAN_device(lua_State *L) {
|
||||||
|
|
||||||
binding_argcheck(L, 1);
|
// Allow : and . access
|
||||||
|
const int arg_offset = (luaL_testudata(L, 1, "CAN") != NULL) ? 1 : 0;
|
||||||
|
|
||||||
const uint32_t raw_buffer_len = coerce_to_uint32_t(L, 1);
|
binding_argcheck(L, 1 + arg_offset);
|
||||||
luaL_argcheck(L, ((raw_buffer_len >= 1U) && (raw_buffer_len <= 25U)), 1, "argument out of range");
|
|
||||||
|
const uint32_t raw_buffer_len = coerce_to_uint32_t(L, 1 + arg_offset);
|
||||||
|
luaL_argcheck(L, ((raw_buffer_len >= 1U) && (raw_buffer_len <= 25U)), 1 + arg_offset, "argument out of range");
|
||||||
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
|
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
|
||||||
|
|
||||||
if (AP::scripting()->_CAN_dev == nullptr) {
|
if (AP::scripting()->_CAN_dev == nullptr) {
|
||||||
@ -337,10 +347,13 @@ int lua_get_CAN_device(lua_State *L) {
|
|||||||
|
|
||||||
int lua_get_CAN_device2(lua_State *L) {
|
int lua_get_CAN_device2(lua_State *L) {
|
||||||
|
|
||||||
binding_argcheck(L, 1);
|
// Allow : and . access
|
||||||
|
const int arg_offset = (luaL_testudata(L, 1, "CAN") != NULL) ? 1 : 0;
|
||||||
|
|
||||||
const uint32_t raw_buffer_len = coerce_to_uint32_t(L, 1);
|
binding_argcheck(L, 1 + arg_offset);
|
||||||
luaL_argcheck(L, ((raw_buffer_len >= 1U) && (raw_buffer_len <= 25U)), 1, "argument out of range");
|
|
||||||
|
const uint32_t raw_buffer_len = coerce_to_uint32_t(L, 1 + arg_offset);
|
||||||
|
luaL_argcheck(L, ((raw_buffer_len >= 1U) && (raw_buffer_len <= 25U)), 1 + arg_offset, "argument out of range");
|
||||||
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
|
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
|
||||||
|
|
||||||
if (AP::scripting()->_CAN_dev2 == nullptr) {
|
if (AP::scripting()->_CAN_dev2 == nullptr) {
|
||||||
|
Loading…
Reference in New Issue
Block a user