5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

AP_Scripting: Support enum types, add millis() remove manual GCS wrapper

This introduces enum types for range checking/returns, adds a millis(() call
(this returns a uint32_t), and moves over to an auto generated version of the GCS binding
This commit is contained in:
Michael du Breuil 2019-04-29 07:42:26 +00:00 committed by WickedShell
parent eddf926433
commit 84c2b18e43
9 changed files with 108 additions and 37 deletions

View File

@ -28,14 +28,18 @@ An example script is given below:
```lua
function update () -- periodic function that will be called
current_pos = Location()
ahrs:get_position(current_pos)
distance = current_pos:get_distance(ahrs:get_home()) -- calculate the distance from home
if distance > 1000 then -- if more then 1000 meters away
distance = 1000; -- clamp the distance to 1000 meters
current_pos = ahrs:get_position()
home = ahrs:get_home()
if current_pos and home then
distance = current_pos:get_distance(ahrs:get_home()) -- calculate the distance from home
if distance > 1000 then -- if more then 1000 meters away
distance = 1000; -- clamp the distance to 1000 meters
end
servo.set_output_pwm(96, 1000 + distance) -- set the servo assigned function 96 (scripting3) to a proportional value
end
servo.set_output_pwm(96, 1000 + distance) -- set the servo assigned function 96 (scripting3) to a proportional value
return update, 1000 -- request to be rerun again 1000 milliseconds (1 second) from now
end
return update, 1000 -- request to be rerun again 1000 milliseconds (1 second) from now
```

View File

@ -120,3 +120,7 @@ singleton AP_Relay method on void uint8_t 0 AP_RELAY_NUM_RELAYS
singleton AP_Relay method off void uint8_t 0 AP_RELAY_NUM_RELAYS
singleton AP_Relay method enabled boolean uint8_t 0 AP_RELAY_NUM_RELAYS
singleton AP_Relay method toggle void uint8_t 0 AP_RELAY_NUM_RELAYS
include GCS_MAVLink/GCS.h
singleton GCS alias gcs
singleton GCS method send_text void MAV_SEVERITY'enum MAV_SEVERITY_EMERGENCY MAV_SEVERITY_DEBUG string

View File

@ -20,6 +20,7 @@ char keyword_userdata[] = "userdata";
char keyword_write[] = "write";
// attributes (should include the leading ' )
char keyword_attr_enum[] = "'enum";
char keyword_attr_null[] = "'Null";
// type keywords
@ -87,6 +88,7 @@ enum field_type {
TYPE_UINT32_T,
TYPE_NONE,
TYPE_STRING,
TYPE_ENUM,
TYPE_USERDATA,
};
@ -99,6 +101,7 @@ const char * type_labels[TYPE_USERDATA + 1] = { "bool",
"uint16_t",
"void",
"string",
"enum",
"userdata",
};
@ -124,6 +127,7 @@ struct range_check {
enum type_flags {
TYPE_FLAGS_NULLABLE = (1U << 1),
TYPE_FLAGS_ENUM = (1U << 2),
};
struct type {
@ -133,6 +137,7 @@ struct type {
uint32_t flags;
union {
char *userdata_name;
char *enum_name;
} data;
};
@ -302,10 +307,10 @@ void string_copy(char **dest, const char * src) {
strcpy(*dest, src);
}
struct range_check *parse_range_check(void) {
struct range_check *parse_range_check(enum field_type type) {
char * low = next_token();
if (low == NULL) {
error(ERROR_USERDATA, "Missing low value for a range check");
error(ERROR_USERDATA, "Missing low value for a range check (type: %s)", type_labels[type]);
}
trace(TRACE_TOKENS, "Range check: Low: %s", low);
@ -346,7 +351,8 @@ unsigned int parse_access_flags(struct type * type) {
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_UINT32_T:
type->range = parse_range_check();
case TYPE_ENUM:
type->range = parse_range_check(type->type);
break;
case TYPE_USERDATA:
case TYPE_BOOLEAN:
@ -405,7 +411,9 @@ int parse_type(struct type *type, const uint32_t restrictions, enum range_check_
char *attribute = strchr(data_type, '\'');
if (attribute != NULL) {
if (strcmp(attribute, keyword_attr_null) == 0) {
if (strcmp(attribute, keyword_attr_enum) == 0) {
type->flags |= TYPE_FLAGS_ENUM;
} else if (strcmp(attribute, keyword_attr_null) == 0) {
if (restrictions & TYPE_RESTRICTION_NOT_NULLABLE) {
error(ERROR_USERDATA, "%s is not nullable in this context", data_type);
}
@ -436,6 +444,9 @@ int parse_type(struct type *type, const uint32_t restrictions, enum range_check_
type->type = TYPE_STRING;
} else if (strcmp(data_type, keyword_void) == 0) {
type->type = TYPE_NONE;
} else if (type->flags & TYPE_FLAGS_ENUM) {
type->type = TYPE_ENUM;
string_copy(&(type->data.enum_name), data_type);
} else {
// assume that this is a user data, we can't validate this until later though
type->type = TYPE_USERDATA;
@ -455,6 +466,7 @@ int parse_type(struct type *type, const uint32_t restrictions, enum range_check_
case TYPE_UINT32_T:
case TYPE_BOOLEAN:
case TYPE_STRING:
case TYPE_ENUM:
case TYPE_USERDATA:
break;
case TYPE_NONE:
@ -473,7 +485,8 @@ int parse_type(struct type *type, const uint32_t restrictions, enum range_check_
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_UINT32_T:
type->range = parse_range_check();
case TYPE_ENUM:
type->range = parse_range_check(type->type);
break;
case TYPE_BOOLEAN:
case TYPE_NONE:
@ -802,6 +815,9 @@ void emit_checker(const struct type t, int arg_number, const char *indentation,
case TYPE_STRING:
fprintf(source, "%schar * data_%d = {};\n", indentation, arg_number);
break;
case TYPE_ENUM:
fprintf(source, "%suint32_t data_%d = {};\n", indentation, arg_number);
break;
case TYPE_USERDATA:
fprintf(source, "%s%s data_%d = {};\n", indentation, t.data.userdata_name, arg_number);
break;
@ -829,6 +845,7 @@ void emit_checker(const struct type t, int arg_number, const char *indentation,
forced_min = "INT16_MIN";
forced_max = "INT16_MAX";
break;
case TYPE_ENUM: // enums are assumed to only ever be within the int32_t space
case TYPE_INT32_T:
forced_min = "INT32_MIN";
forced_max = "INT32_MAX";
@ -865,6 +882,7 @@ void emit_checker(const struct type t, int arg_number, const char *indentation,
case TYPE_INT32_T:
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_ENUM:
fprintf(source, "%sconst lua_Integer raw_data_%d = luaL_checkinteger(L, %d);\n", indentation, arg_number, arg_number);
break;
case TYPE_UINT32_T:
@ -918,6 +936,9 @@ void emit_checker(const struct type t, int arg_number, const char *indentation,
case TYPE_STRING:
fprintf(source, "%sconst char * data_%d = luaL_checkstring(L, %d);\n", indentation, arg_number, arg_number);
break;
case TYPE_ENUM:
fprintf(source, "%sconst %s data_%d = static_cast<%s>(raw_data_%d);\n", indentation, t.data.enum_name, arg_number, t.data.enum_name, arg_number);
break;
case TYPE_USERDATA:
fprintf(source, "%s%s & data_%d = *check_%s(L, %d);\n", indentation, t.data.userdata_name, arg_number, t.data.userdata_name, arg_number);
break;
@ -947,6 +968,7 @@ void emit_userdata_field(const struct userdata *data, const struct userdata_fiel
case TYPE_INT32_T:
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_ENUM:
fprintf(source, " lua_pushinteger(L, ud->%s);\n", field->name);
break;
case TYPE_UINT32_T:
@ -1001,7 +1023,8 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
// emit comments on expected arg/type
struct argument *arg = method->arguments;
while (arg != NULL) {
fprintf(source, " // %d %s %d : %d\n", arg_count++, arg->type.type == TYPE_USERDATA ? arg->type.data.userdata_name : type_labels[arg->type.type],
fprintf(source, " // %d %s %d : %d\n", arg_count++, arg->type.type == TYPE_USERDATA ? arg->type.data.userdata_name :
arg->type.type == TYPE_ENUM ? arg->type.data.enum_name : type_labels[arg->type.type],
arg->line_num, arg->token_num);
arg = arg->next;
}
@ -1077,6 +1100,9 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
case TYPE_UINT32_T:
fprintf(source, " const uint32_t data = ud->%s(\n", method->name);
break;
case TYPE_ENUM:
fprintf(source, " const %s &data = ud->%s(\n", method->return_type.data.enum_name, method->name);
break;
case TYPE_USERDATA:
fprintf(source, " const %s &data = ud->%s(\n", method->return_type.data.userdata_name, method->name);
break;
@ -1125,6 +1151,7 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
case TYPE_INT32_T:
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_ENUM:
fprintf(source, " lua_pushinteger(L, data_%d);\n", arg_index);
break;
case TYPE_UINT32_T:
@ -1163,6 +1190,7 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
case TYPE_INT32_T:
case TYPE_UINT8_T:
case TYPE_UINT16_T:
case TYPE_ENUM:
fprintf(source, " lua_pushinteger(L, data);\n");
break;
case TYPE_UINT32_T:

View File

@ -1,12 +1,14 @@
#include <AP_Common/AP_Common.h>
#include <GCS_MAVLink/GCS.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Common/Location.h>
#include <AP_HAL/HAL.h>
#include "lua_bindings.h"
#include "lua_boxed_numerics.h"
#include "lua_generated_bindings.h"
extern const AP_HAL::HAL& hal;
int check_arguments(lua_State *L, int expected_arguments, const char *fn_name);
int check_arguments(lua_State *L, int expected_arguments, const char *fn_name) {
#if defined(AP_SCRIPTING_CHECKS) && AP_SCRIPTING_CHECKS >= 1
@ -22,24 +24,6 @@ int check_arguments(lua_State *L, int expected_arguments, const char *fn_name) {
return 0;
}
// GCS binding
int lua_gcs_send_text(lua_State *L);
int lua_gcs_send_text(lua_State *L) {
check_arguments(L, 1, "send_text");
const char* str = luaL_checkstring(L, -1);
gcs().send_text(MAV_SEVERITY_INFO, str);
return 0;
}
static const luaL_Reg gcs_functions[] =
{
{"send_text", lua_gcs_send_text},
{NULL, NULL}
};
// servo binding
int lua_servo_set_output_pwm(lua_State *L);
@ -59,6 +43,16 @@ int lua_servo_set_output_pwm(lua_State *L) {
return 0;
}
// millis
int lua_millis(lua_State *L) {
check_arguments(L, 0, "millis");
new_uint32_t(L);
*check_uint32_t(L, -1) = AP_HAL::millis();
return 1;
}
static const luaL_Reg servo_functions[] =
{
{"set_output_pwm", lua_servo_set_output_pwm},
@ -66,12 +60,12 @@ static const luaL_Reg servo_functions[] =
};
void load_lua_bindings(lua_State *L) {
luaL_newlib(L, gcs_functions);
lua_setglobal(L, "gcs");
luaL_newlib(L, servo_functions);
lua_setglobal(L, "servo");
load_generated_bindings(L);
lua_pushcfunction(L, lua_millis);
lua_setglobal(L, "millis");
}

View File

@ -11,6 +11,11 @@ int new_uint32_t(lua_State *L) {
return 1;
}
uint32_t * check_uint32_t(lua_State *L, int arg) {
void *data = luaL_checkudata(L, arg, "uint32_t");
return static_cast<uint32_t *>(data);
}
uint32_t coerce_to_uint32_t(lua_State *L, int arg) {
{ // userdata
const uint32_t * ud = static_cast<uint32_t *>(luaL_testudata(L, arg, "uint32_t"));

View File

@ -3,6 +3,7 @@
#include "lua/src/lua.hpp"
int new_uint32_t(lua_State *L);
uint32_t *check_uint32_t(lua_State *L, int arg);
void load_boxed_numerics(lua_State *L);
void load_boxed_numerics_sandbox(lua_State *L);

View File

@ -1,6 +1,7 @@
// auto generated bindings, don't manually edit
#include "lua_generated_bindings.h"
#include "lua_boxed_numerics.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Relay/AP_Relay.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
@ -559,6 +560,32 @@ const luaL_Reg Location_meta[] = {
{NULL, NULL}
};
static int GCS_send_text(lua_State *L) {
// 1 MAV_SEVERITY 126 : 8
// 2 enum 126 : 9
const int args = lua_gettop(L);
if (args > 3) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
return luaL_argerror(L, args, "too few arguments");
}
GCS * ud = GCS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gcs not supported on this firmware");
}
const lua_Integer raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(MAV_SEVERITY_EMERGENCY, INT32_MIN)) && (raw_data_2 <= MIN(MAV_SEVERITY_DEBUG, INT32_MAX))), 2, "argument out of range");
const MAV_SEVERITY data_2 = static_cast<MAV_SEVERITY>(raw_data_2);
const char * data_3 = luaL_checkstring(L, 3);
ud->send_text(
data_2,
data_3);
return 0;
}
static int AP_Relay_toggle(lua_State *L) {
// 1 uint8_t 122 : 8
const int args = lua_gettop(L);
@ -837,7 +864,7 @@ static int RangeFinder_num_sensors(lua_State *L) {
}
static int AP_Notify_play_tune(lua_State *L) {
// 1 userdata 99 : 6
// 1 enum 99 : 6
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
@ -1958,6 +1985,11 @@ static int AP_AHRS_get_position(lua_State *L) {
return 1;
}
const luaL_Reg GCS_meta[] = {
{"send_text", GCS_send_text},
{NULL, NULL}
};
const luaL_Reg AP_Relay_meta[] = {
{"toggle", AP_Relay_toggle},
{"enabled", AP_Relay_enabled},
@ -2060,6 +2092,7 @@ const struct singleton_fun {
const char *name;
const luaL_Reg *reg;
} singleton_fun[] = {
{"gcs", GCS_meta},
{"relay", AP_Relay_meta},
{"terrain", AP_Terrain_meta},
{"rangefinder", RangeFinder_meta},
@ -2100,6 +2133,7 @@ void load_generated_bindings(lua_State *L) {
}
const char *singletons[] = {
"gcs",
"relay",
"terrain",
"rangefinder",

View File

@ -1,5 +1,6 @@
#pragma once
// auto generated bindings, don't manually edit
#include <GCS_MAVLink/GCS.h>
#include <AP_Relay/AP_Relay.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RangeFinder/AP_RangeFinder.h>

View File

@ -30,7 +30,7 @@ function get_sandbox_env ()
codepoint = utf8.codepoint, len = utf8.len, offsets = utf8.offsets},
-- ArduPilot specific
gcs = { send_text = gcs.send_text},
millis = millis,
servo = { set_output_pwm = servo.set_output_pwm},
}
end