AP_Scripting: Add operator overloading

Also introduces battmonitor wrappers, and removes an unneeded checkdata
This commit is contained in:
Michael du Breuil 2019-04-21 17:34:52 -07:00 committed by Andrew Tridgell
parent e6d97f63a5
commit 1b48ce57f0
4 changed files with 607 additions and 134 deletions

View File

@ -76,6 +76,8 @@ userdata Vector3f method normalize void
userdata Vector3f method is_nan void
userdata Vector3f method is_inf void
userdata Vector3f method is_zero void
userdata Vector3f operator +
userdata Vector3f operator -
userdata Vector2f field x float read write -FLT_MAX FLT_MAX
userdata Vector2f field y float read write -FLT_MAX FLT_MAX
@ -84,6 +86,8 @@ userdata Vector2f method normalize void
userdata Vector2f method is_nan void
userdata Vector2f method is_inf void
userdata Vector2f method is_zero void
userdata Vector2f operator +
userdata Vector2f operator -
include AP_Notify/AP_Notify.h
singleton notify alias notify

View File

@ -12,6 +12,7 @@ char keyword_comment[] = "--";
char keyword_field[] = "field";
char keyword_include[] = "include";
char keyword_method[] = "method";
char keyword_operator[] = "operator";
char keyword_read[] = "read";
char keyword_singleton[] = "singleton";
char keyword_userdata[] = "userdata";
@ -98,6 +99,14 @@ const char * type_labels[TYPE_USERDATA + 1] = { "bool",
"userdata",
};
enum operator_type {
OP_ADD = (1U << 0),
OP_SUB = (1U << 1),
OP_MUL = (1U << 2),
OP_DIV = (1U << 3),
OP_LAST
};
enum access_type {
ACCESS_VALUE = 0,
ACCESS_REFERENCE,
@ -274,6 +283,7 @@ struct userdata {
struct userdata_field *fields;
struct method *methods;
enum userdata_type ud_type;
uint32_t operations; // bitset of enum operation_types
};
static struct userdata *parsed_userdata = NULL;
@ -491,8 +501,8 @@ void handle_userdata_field(struct userdata *data) {
field->access_flags = parse_access_flags(&(field->type));
}
void handle_method(enum trace_level traceType, char *parent_name, struct method **methods) {
trace(traceType, "Adding a method");
void handle_method(char *parent_name, struct method **methods) {
trace(TRACE_USERDATA, "Adding a method");
// find the field name
char * name = next_token();
@ -508,7 +518,7 @@ void handle_method(enum trace_level traceType, char *parent_name, struct method
error(ERROR_USERDATA, "Method %s already exsists for %s (declared on %d)", name, parent_name, method->line);
}
trace(traceType, "Adding method %s", name);
trace(TRACE_USERDATA, "Adding method %s", name);
method = allocate(sizeof(struct method));
method->next = *methods;
*methods = method;
@ -548,6 +558,44 @@ void handle_method(enum trace_level traceType, char *parent_name, struct method
}
}
void handle_operator(struct userdata *data) {
trace(TRACE_USERDATA, "Adding a operator");
if (data->ud_type != UD_USERDATA) {
error(ERROR_USERDATA, "Operators are only allowed on userdata objects");
}
char *operator = next_token();
if (operator == NULL) {
error(ERROR_USERDATA, "Needed a symbol for the operator");
}
enum operator_type operation;
if (strcmp(operator, "+") == 0) {
operation = OP_ADD;
} else if (strcmp(operator, "-") == 0) {
operation = OP_SUB;
} else if (strcmp(operator, "*") == 0) {
operation = OP_MUL;
} else if (strcmp(operator, "/") == 0) {
operation = OP_DIV;
} else {
error(ERROR_USERDATA, "Unknown operation type: %s", operator);
}
if ((data->operations) & operation) {
error(ERROR_USERDATA, "Operation %s was already defined for %s", operator, data->name);
}
trace(TRACE_USERDATA, "Adding operation %d to %s", operation, data->name);
data->operations |= operation;
if (next_token() != NULL) {
error(ERROR_USERDATA, "Extra token on operation %s", operator);
}
}
void handle_userdata(void) {
trace(TRACE_USERDATA, "Adding a userdata");
@ -581,8 +629,10 @@ void handle_userdata(void) {
// match type
if (strcmp(type, keyword_field) == 0) {
handle_userdata_field(node);
} else if (strcmp(type, keyword_operator) == 0) {
handle_operator(node);
} else if (strcmp(type, keyword_method) == 0) {
handle_method(TRACE_USERDATA, node->name, &(node->methods));
handle_method(node->name, &(node->methods));
} else {
error(ERROR_USERDATA, "Unknown or unsupported type for userdata: %s", type);
}
@ -637,7 +687,7 @@ void handle_singleton(void) {
}
return;
} else if (strcmp(type, keyword_method) == 0) {
handle_method(TRACE_USERDATA, node->name, &(node->methods));
handle_method(node->name, &(node->methods));
} else {
error(ERROR_SINGLETON, "Singletons only support methods or aliases (got %s)", type);
}
@ -925,6 +975,7 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
// sanity check number of args called with
fprintf(source, " const int args = lua_gettop(L);\n");
arg = method->arguments;
arg_count = 1;
while (arg != NULL) {
if (!(arg->type.flags & TYPE_FLAGS_NULLABLE)) {
arg_count++;
@ -936,7 +987,6 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
fprintf(source, " } else if (args < %d) {\n", arg_count);
fprintf(source, " return luaL_argerror(L, args, \"too few arguments\");\n");
fprintf(source, " }\n\n");
fprintf(source, " luaL_checkudata(L, 1, \"%s\");\n\n", access_name);
switch (data->ud_type) {
case UD_USERDATA:
@ -1087,13 +1137,87 @@ void emit_userdata_method(const struct userdata *data, const struct method *meth
fprintf(source, "}\n\n");
}
const char * get_name_for_operation(enum operator_type op) {
switch (op) {
case OP_ADD:
return "__add";
case OP_SUB:
return "__sub";
case OP_MUL:
return "__mul";
break;
case OP_DIV:
return "__div";
break;
case OP_LAST:
return NULL;
}
return NULL;
}
void emit_operators(struct userdata *data) {
trace(TRACE_USERDATA, "Emitting operators for %s", data->name);
assert(data->ud_type == UD_USERDATA);
for (uint32_t i = 1; i < OP_LAST; i = (i << 1)) {
const char * op_name = get_name_for_operation((data->operations) & i);
if (op_name == NULL) {
continue;
}
char op_sym;
switch ((data->operations) & i) {
case OP_ADD:
op_sym = '+';
break;
case OP_SUB:
op_sym = '-';
break;
case OP_MUL:
op_sym = '*';
break;
case OP_DIV:
op_sym = '/';
break;
case OP_LAST:
return;
}
fprintf(source, "static int %s_%s(lua_State *L) {\n", data->name, op_name);
// check number of arguments
fprintf(source, " const int args = lua_gettop(L);\n");
fprintf(source, " if (args > 2) {\n");
fprintf(source, " return luaL_argerror(L, args, \"too many arguments\");\n");
fprintf(source, " } else if (args < 2) {\n");
fprintf(source, " return luaL_argerror(L, args, \"too few arguments\");\n");
fprintf(source, " }\n\n");
// check the pointers
fprintf(source, " %s *ud = check_%s(L, 1);\n", data->name, data->name);
fprintf(source, " %s *ud2 = check_%s(L, 2);\n", data->name, data->name);
// create a container for the result
fprintf(source, " new_%s(L);\n", data->name);
fprintf(source, " *check_%s(L, -1) = *ud %c *ud2;;\n", data->name, op_sym);
// return the first pointer
fprintf(source, " return 1;\n");
fprintf(source, "}\n\n");
}
}
void emit_userdata_methods(struct userdata *node) {
while(node) {
// methods
struct method *method = node->methods;
while(method) {
emit_userdata_method(node, method);
method = method->next;
}
// operators
if (node->operations) {
emit_operators(node);
}
node = node->next;
}
}
@ -1115,6 +1239,14 @@ void emit_userdata_metatables(void) {
method = method->next;
}
for (uint32_t i = 1; i < OP_LAST; i = i << 1) {
const char * op_name = get_name_for_operation((node->operations) & i);
if (op_name == NULL) {
continue;
}
fprintf(source, " {\"%s\", %s_%s},\n", op_name, node->name, op_name);
}
fprintf(source, " {NULL, NULL}\n");
fprintf(source, "};\n\n");

View File

@ -4,6 +4,7 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_Math/AP_Math.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/Location.h>
@ -245,8 +246,6 @@ static int Vector2f_is_zero(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector2f");
Vector2f * ud = check_Vector2f(L, 1);
ud->is_zero(
);
@ -262,8 +261,6 @@ static int Vector2f_is_inf(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector2f");
Vector2f * ud = check_Vector2f(L, 1);
ud->is_inf(
);
@ -279,8 +276,6 @@ static int Vector2f_is_nan(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector2f");
Vector2f * ud = check_Vector2f(L, 1);
ud->is_nan(
);
@ -296,8 +291,6 @@ static int Vector2f_normalize(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector2f");
Vector2f * ud = check_Vector2f(L, 1);
ud->normalize(
);
@ -313,8 +306,6 @@ static int Vector2f_length(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector2f");
Vector2f * ud = check_Vector2f(L, 1);
const float data = ud->length(
);
@ -323,6 +314,36 @@ static int Vector2f_length(lua_State *L) {
return 1;
}
static int Vector2f___add(lua_State *L) {
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
Vector2f *ud = check_Vector2f(L, 1);
Vector2f *ud2 = check_Vector2f(L, 2);
new_Vector2f(L);
*check_Vector2f(L, -1) = *ud + *ud2;;
return 1;
}
static int Vector2f___sub(lua_State *L) {
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
Vector2f *ud = check_Vector2f(L, 1);
Vector2f *ud2 = check_Vector2f(L, 2);
new_Vector2f(L);
*check_Vector2f(L, -1) = *ud - *ud2;;
return 1;
}
static int Vector3f_is_zero(lua_State *L) {
const int args = lua_gettop(L);
if (args > 1) {
@ -331,8 +352,6 @@ static int Vector3f_is_zero(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector3f");
Vector3f * ud = check_Vector3f(L, 1);
ud->is_zero(
);
@ -348,8 +367,6 @@ static int Vector3f_is_inf(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector3f");
Vector3f * ud = check_Vector3f(L, 1);
ud->is_inf(
);
@ -365,8 +382,6 @@ static int Vector3f_is_nan(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector3f");
Vector3f * ud = check_Vector3f(L, 1);
ud->is_nan(
);
@ -382,8 +397,6 @@ static int Vector3f_normalize(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector3f");
Vector3f * ud = check_Vector3f(L, 1);
ud->normalize(
);
@ -399,8 +412,6 @@ static int Vector3f_length(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Vector3f");
Vector3f * ud = check_Vector3f(L, 1);
const float data = ud->length(
);
@ -409,16 +420,44 @@ static int Vector3f_length(lua_State *L) {
return 1;
}
static int Location_get_vector_from_origin_NEU(lua_State *L) {
// 1 Vector3f 14 : 6
static int Vector3f___add(lua_State *L) {
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Location");
Vector3f *ud = check_Vector3f(L, 1);
Vector3f *ud2 = check_Vector3f(L, 2);
new_Vector3f(L);
*check_Vector3f(L, -1) = *ud + *ud2;;
return 1;
}
static int Vector3f___sub(lua_State *L) {
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
Vector3f *ud = check_Vector3f(L, 1);
Vector3f *ud2 = check_Vector3f(L, 2);
new_Vector3f(L);
*check_Vector3f(L, -1) = *ud - *ud2;;
return 1;
}
static int Location_get_vector_from_origin_NEU(lua_State *L) {
// 1 Vector3f 14 : 6
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
Location * ud = check_Location(L, 1);
Vector3f & data_2 = *check_Vector3f(L, 2);
@ -433,14 +472,12 @@ static int Location_offset(lua_State *L) {
// 1 float 13 : 8
// 2 float 13 : 11
const int args = lua_gettop(L);
if (args > 5) {
if (args > 3) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 5) {
} else if (args < 3) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Location");
Location * ud = check_Location(L, 1);
const float raw_data_2 = luaL_checknumber(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "argument out of range");
@ -458,14 +495,12 @@ static int Location_offset(lua_State *L) {
static int Location_get_distance(lua_State *L) {
// 1 Location 12 : 6
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "Location");
Location * ud = check_Location(L, 1);
Location & data_2 = *check_Location(L, 2);
const float data = ud->get_distance(
@ -483,6 +518,8 @@ const luaL_Reg Vector2f_meta[] = {
{"is_nan", Vector2f_is_nan},
{"normalize", Vector2f_normalize},
{"length", Vector2f_length},
{"__add", Vector2f___add},
{"__sub", Vector2f___sub},
{NULL, NULL}
};
@ -495,6 +532,8 @@ const luaL_Reg Vector3f_meta[] = {
{"is_nan", Vector3f_is_nan},
{"normalize", Vector3f_normalize},
{"length", Vector3f_length},
{"__add", Vector3f___add},
{"__sub", Vector3f___sub},
{NULL, NULL}
};
@ -519,8 +558,6 @@ static int RangeFinder_num_sensors(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "rangefinder");
RangeFinder * ud = RangeFinder::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "rangefinder not supported on this firmware");
@ -534,16 +571,14 @@ static int RangeFinder_num_sensors(lua_State *L) {
}
static int AP_Notify_play_tune(lua_State *L) {
// 1 string 90 : 6
// 1 string 94 : 6
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "AP_Notify");
AP_Notify * ud = AP_Notify::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "AP_Notify not supported on this firmware");
@ -564,8 +599,6 @@ static int AP_GPS_first_unconfigured_gps(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -586,8 +619,6 @@ static int AP_GPS_all_configured(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -603,14 +634,12 @@ static int AP_GPS_all_configured(lua_State *L) {
static int AP_GPS_get_antenna_offset(lua_State *L) {
// 1 uint8_t 65 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -630,14 +659,12 @@ static int AP_GPS_get_antenna_offset(lua_State *L) {
static int AP_GPS_have_vertical_velocity(lua_State *L) {
// 1 uint8_t 64 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -656,14 +683,12 @@ static int AP_GPS_have_vertical_velocity(lua_State *L) {
static int AP_GPS_get_vdop(lua_State *L) {
// 1 uint8_t 63 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -682,14 +707,12 @@ static int AP_GPS_get_vdop(lua_State *L) {
static int AP_GPS_get_hdop(lua_State *L) {
// 1 uint8_t 62 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -708,14 +731,12 @@ static int AP_GPS_get_hdop(lua_State *L) {
static int AP_GPS_num_sats(lua_State *L) {
// 1 uint8_t 61 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -734,14 +755,12 @@ static int AP_GPS_num_sats(lua_State *L) {
static int AP_GPS_ground_course(lua_State *L) {
// 1 uint8_t 60 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -760,14 +779,12 @@ static int AP_GPS_ground_course(lua_State *L) {
static int AP_GPS_ground_speed(lua_State *L) {
// 1 uint8_t 59 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -786,14 +803,12 @@ static int AP_GPS_ground_speed(lua_State *L) {
static int AP_GPS_velocity(lua_State *L) {
// 1 uint8_t 58 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -814,14 +829,12 @@ static int AP_GPS_vertical_accuracy(lua_State *L) {
// 1 uint8_t 57 : 8
// 2 float 57 : 9
const int args = lua_gettop(L);
if (args > 4) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 4) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -847,14 +860,12 @@ static int AP_GPS_horizontal_accuracy(lua_State *L) {
// 1 uint8_t 56 : 8
// 2 float 56 : 9
const int args = lua_gettop(L);
if (args > 4) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 4) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -880,14 +891,12 @@ static int AP_GPS_speed_accuracy(lua_State *L) {
// 1 uint8_t 55 : 8
// 2 float 55 : 9
const int args = lua_gettop(L);
if (args > 4) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 4) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -912,14 +921,12 @@ static int AP_GPS_speed_accuracy(lua_State *L) {
static int AP_GPS_location(lua_State *L) {
// 1 uint8_t 54 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -939,14 +946,12 @@ static int AP_GPS_location(lua_State *L) {
static int AP_GPS_status(lua_State *L) {
// 1 uint8_t 53 : 8
const int args = lua_gettop(L);
if (args > 3) {
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 3) {
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -970,8 +975,6 @@ static int AP_GPS_primary_sensor(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -992,8 +995,6 @@ static int AP_GPS_num_sensors(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "gps");
AP_GPS * ud = AP_GPS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "gps not supported on this firmware");
@ -1006,6 +1007,341 @@ static int AP_GPS_num_sensors(lua_State *L) {
return 1;
}
static int AP_BattMonitor_get_temperature(lua_State *L) {
// 1 float 46 : 6
// 2 uint8_t 46 : 9
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
float data_5002 = {};
const int raw_data_3 = luaL_checkinteger(L, 3);
luaL_argcheck(L, ((raw_data_3 >= MAX(0, 0)) && (raw_data_3 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 3, "argument out of range");
const uint8_t data_3 = static_cast<uint8_t>(raw_data_3);
const bool data = ud->get_temperature(
data_5002,
data_3);
if (data) {
lua_pushnumber(L, data_5002);
} else {
lua_pushnil(L);
}
return 1;
}
static int AP_BattMonitor_overpower_detected(lua_State *L) {
// 1 uint8_t 45 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const bool data = ud->overpower_detected(
data_2);
lua_pushboolean(L, data);
return 1;
}
static int AP_BattMonitor_has_failsafed(lua_State *L) {
const int args = lua_gettop(L);
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const bool data = ud->has_failsafed(
);
lua_pushboolean(L, data);
return 1;
}
static int AP_BattMonitor_pack_capacity_mah(lua_State *L) {
// 1 uint8_t 43 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const int32_t data = ud->pack_capacity_mah(
data_2);
lua_pushinteger(L, data);
return 1;
}
static int AP_BattMonitor_capacity_remaining_pct(lua_State *L) {
// 1 uint8_t 42 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const uint8_t data = ud->capacity_remaining_pct(
data_2);
lua_pushinteger(L, data);
return 1;
}
static int AP_BattMonitor_consumed_wh(lua_State *L) {
// 1 uint8_t 41 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const float data = ud->consumed_wh(
data_2);
lua_pushnumber(L, data);
return 1;
}
static int AP_BattMonitor_consumed_mah(lua_State *L) {
// 1 uint8_t 40 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const float data = ud->consumed_mah(
data_2);
lua_pushnumber(L, data);
return 1;
}
static int AP_BattMonitor_current_amps(lua_State *L) {
// 1 uint8_t 39 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const float data = ud->current_amps(
data_2);
lua_pushnumber(L, data);
return 1;
}
static int AP_BattMonitor_voltage_resting_estimate(lua_State *L) {
// 1 uint8_t 38 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const float data = ud->voltage_resting_estimate(
data_2);
lua_pushnumber(L, data);
return 1;
}
static int AP_BattMonitor_voltage(lua_State *L) {
// 1 uint8_t 37 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const float data = ud->voltage(
data_2);
lua_pushnumber(L, data);
return 1;
}
static int AP_BattMonitor_has_current(lua_State *L) {
// 1 uint8_t 36 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const bool data = ud->has_current(
data_2);
lua_pushboolean(L, data);
return 1;
}
static int AP_BattMonitor_has_consumed_energy(lua_State *L) {
// 1 uint8_t 35 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const bool data = ud->has_consumed_energy(
data_2);
lua_pushboolean(L, data);
return 1;
}
static int AP_BattMonitor_healthy(lua_State *L) {
// 1 uint8_t 34 : 8
const int args = lua_gettop(L);
if (args > 2) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const int raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(AP_BATT_MONITOR_MAX_INSTANCES, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const bool data = ud->healthy(
data_2);
lua_pushboolean(L, data);
return 1;
}
static int AP_BattMonitor_num_instances(lua_State *L) {
const int args = lua_gettop(L);
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
AP_BattMonitor * ud = AP_BattMonitor::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "battery not supported on this firmware");
}
const uint8_t data = ud->num_instances(
);
lua_pushinteger(L, data);
return 1;
}
static int AP_AHRS_prearm_healthy(lua_State *L) {
const int args = lua_gettop(L);
if (args > 1) {
@ -1014,8 +1350,6 @@ static int AP_AHRS_prearm_healthy(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1036,8 +1370,6 @@ static int AP_AHRS_home_is_set(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1053,14 +1385,12 @@ static int AP_AHRS_home_is_set(lua_State *L) {
static int AP_AHRS_get_relative_position_NED_home(lua_State *L) {
// 1 Vector3f 26 : 6
const int args = lua_gettop(L);
if (args > 2) {
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1082,14 +1412,12 @@ static int AP_AHRS_get_relative_position_NED_home(lua_State *L) {
static int AP_AHRS_get_velocity_NED(lua_State *L) {
// 1 Vector3f 25 : 6
const int args = lua_gettop(L);
if (args > 2) {
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1116,8 +1444,6 @@ static int AP_AHRS_groundspeed_vector(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1139,8 +1465,6 @@ static int AP_AHRS_wind_estimate(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1157,14 +1481,12 @@ static int AP_AHRS_wind_estimate(lua_State *L) {
static int AP_AHRS_get_hagl(lua_State *L) {
// 1 float 22 : 6
const int args = lua_gettop(L);
if (args > 2) {
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1190,8 +1512,6 @@ static int AP_AHRS_get_gyro(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1213,8 +1533,6 @@ static int AP_AHRS_get_home(lua_State *L) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1231,14 +1549,12 @@ static int AP_AHRS_get_home(lua_State *L) {
static int AP_AHRS_get_position(lua_State *L) {
// 1 Location 19 : 6
const int args = lua_gettop(L);
if (args > 2) {
if (args > 1) {
return luaL_argerror(L, args, "too many arguments");
} else if (args < 2) {
} else if (args < 1) {
return luaL_argerror(L, args, "too few arguments");
}
luaL_checkudata(L, 1, "ahrs");
AP_AHRS * ud = AP_AHRS::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, args, "ahrs not supported on this firmware");
@ -1292,6 +1608,24 @@ const luaL_Reg AP_GPS_meta[] = {
{NULL, NULL}
};
const luaL_Reg AP_BattMonitor_meta[] = {
{"get_temperature", AP_BattMonitor_get_temperature},
{"overpower_detected", AP_BattMonitor_overpower_detected},
{"has_failsafed", AP_BattMonitor_has_failsafed},
{"pack_capacity_mah", AP_BattMonitor_pack_capacity_mah},
{"capacity_remaining_pct", AP_BattMonitor_capacity_remaining_pct},
{"consumed_wh", AP_BattMonitor_consumed_wh},
{"consumed_mah", AP_BattMonitor_consumed_mah},
{"current_amps", AP_BattMonitor_current_amps},
{"voltage_resting_estimate", AP_BattMonitor_voltage_resting_estimate},
{"voltage", AP_BattMonitor_voltage},
{"has_current", AP_BattMonitor_has_current},
{"has_consumed_energy", AP_BattMonitor_has_consumed_energy},
{"healthy", AP_BattMonitor_healthy},
{"num_instances", AP_BattMonitor_num_instances},
{NULL, NULL}
};
const luaL_Reg AP_AHRS_meta[] = {
{"prearm_healthy", AP_AHRS_prearm_healthy},
{"home_is_set", AP_AHRS_home_is_set},
@ -1323,6 +1657,7 @@ const struct singleton_fun {
{"AP_Notify", AP_Notify_meta},
{"notify", notify_meta},
{"gps", AP_GPS_meta},
{"battery", AP_BattMonitor_meta},
{"ahrs", AP_AHRS_meta},
};
@ -1357,6 +1692,7 @@ const char *singletons[] = {
"AP_Notify",
"notify",
"gps",
"battery",
"ahrs",
};

View File

@ -4,6 +4,7 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_Math/AP_Math.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/Location.h>
#include "lua/src/lua.hpp"