Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: Add location and ahrs user data

Browse Source
This commit is contained in:
Michael du Breuil 2019-01-21 16:00:57 -07:00 committed by WickedShell
parent 9dd36ba511
commit b45970fded
4 changed files with 371 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Scripting/README.md Normal file
View File

@ -0,0 +1,41 @@
# AP_Scripting
## Enabling Scripting Support in Builds
To enable scripting the `--enable-scripting` flag must be passed to waf.
The following example enables scripting and builds the ArduPlane firmware for the Cube.
```
$ waf configure --enable-scripting --board=CubeBlack
$ waf plane
```
To run SITL you can simply use the `sim_vehicle.py` script which will wrap the configuration, compilation,
and launching of the simulation into one command for you.
```
$ Tools/autotest/sim_vehicle.py --waf-configure-arg --enable-scripting -v ArduPlane
```
## Adding Scripts
The vehicle will automatically look for and launch any scripts that are contained in the `scripts` folder when it starts.
On real hardware this should be inside of the `APM` folder of the SD card. In SITL this should be in the working directory (typically the main `ardupilot` directory).
An example script is given below:
```lua
function update () -- periodic function that will be called
distance = ahrs:position():distance(ahrs: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
return update, 1000 -- request to be rerun again 1000 milliseconds (1 second) from now
end
return update, 0 -- immediately run the update function
```

337
libraries/AP_Scripting/lua_bindings.cpp
View File

@ -4,7 +4,14 @@
#include "lua_bindings.h"
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
if (expected_arguments < 0) {
AP_HAL::panic("Lua: Attempted to check for negative arguments");
}
#endif
const int args = lua_gettop(L);
if (args != expected_arguments) {
return luaL_error(L, "%s expected %d arguments got %d", fn_name, expected_arguments, args);
@ -14,10 +21,11 @@ int check_arguments(lua_State *L, int expected_arguments, const char *fn_name) {
// 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 = lua_tostring(L, 1);
const char* str = lua_tostring(L, -1);
gcs().send_text(MAV_SEVERITY_INFO, str);
return 0;
@ -31,15 +39,16 @@ static const luaL_Reg gcs_functions[] =
// servo binding
int lua_servo_set_output_pwm(lua_State *L);
int lua_servo_set_output_pwm(lua_State *L) {
check_arguments(L, 2, "set_output_pwm");
const SRV_Channel::Aux_servo_function_t servo_function = (SRV_Channel::Aux_servo_function_t)luaL_checkinteger(L, 1);
const SRV_Channel::Aux_servo_function_t servo_function = (SRV_Channel::Aux_servo_function_t)luaL_checkinteger(L, -2);
luaL_argcheck(L, ((servo_function >= SRV_Channel::Aux_servo_function_t::k_scripting1) &&
(servo_function <= SRV_Channel::Aux_servo_function_t::k_scripting16)),
2, "function out of range");
const int output = luaL_checknumber(L, 2);
const int output = luaL_checknumber(L, -1);
luaL_argcheck(L, ((output >= 0) && (output <= UINT16_MAX)), 2, "output out of range");
SRV_Channels::set_output_pwm((SRV_Channel::Aux_servo_function_t)servo_function, output);
@ -53,12 +62,320 @@ static const luaL_Reg servo_functions[] =
{NULL, NULL}
};
// all bindings
void load_lua_bindings(lua_State *state) {
luaL_newlib(state, gcs_functions);
lua_setglobal(state, "gcs");
luaL_newlib(state, servo_functions);
lua_setglobal(state, "servo");
// location stuff
static int new_location(lua_State *L) {
Location *loc = (Location *)lua_newuserdata(L, sizeof(Location));
luaL_getmetatable(L, "location");
lua_setmetatable(L, -2);
memset((void *)loc, 0, sizeof(Location));
return 1;
}
static Location *check_location(lua_State *L, int arg) {
void *data = luaL_checkudata(L, arg, "location");
luaL_argcheck(L, data != NULL, arg, "`location` expected");
return (Location *)data;
}
static int location_lat(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushinteger(L, loc->lat);
return 1;
case 2: // set
{
const int32_t lat = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((lat >= -900000000) && (lat <= 900000000)), 2, "invalid latitude out of range");
loc->lat = lat;
return 0;
}
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_long(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushinteger(L, loc->lng);
return 1;
case 2: // set
{
const int32_t lng = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((lng >= -1800000000) && (lng <= 1800000000)), 2, "invalid longitude out of range");
loc->lng = lng;
return 0;
}
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_alt(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushinteger(L, loc->alt);
return 1;
case 2: // set
loc->alt = luaL_checknumber(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_relative_alt(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushboolean(L, loc->relative_alt);
return 1;
case 2: // set
loc->relative_alt = lua_toboolean(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_loiter_ccw(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushboolean(L, loc->loiter_ccw);
return 1;
case 2: // set
loc->loiter_ccw = lua_toboolean(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_terrain_alt(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushboolean(L, loc->terrain_alt);
return 1;
case 2: // set
loc->terrain_alt = lua_toboolean(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_origin_alt(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushboolean(L, loc->origin_alt);
return 1;
case 2: // set
loc->origin_alt = lua_toboolean(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_loiter_xtrack(lua_State *L) {
Location *loc = check_location(L, 1);
switch(lua_gettop(L)) {
case 1: // access
lua_pushboolean(L, loc->loiter_xtrack);
return 1;
case 2: // set
loc->loiter_xtrack = lua_toboolean(L, 2);
return 0;
default:
return luaL_argerror(L, lua_gettop(L), "too many arguments");
}
}
static int location_tostring(lua_State *L) {
Location *loc = check_location(L, -1);
char buf[64] = {};
snprintf(buf, sizeof(buf),
"Loc(%ld.%07ld %c, %ld.%07ld %c, %.02f)",
labs(loc->lat / 10000000),
labs(loc->lat % 10000000),
loc->lat > 0 ? 'N' : 'S',
labs(loc->lng / 10000000),
labs(loc->lng % 10000000),
loc->lng > 0 ? 'E' : 'W',
loc->alt * 1e-2);
lua_pushstring(L, buf);
return 1;
}
static int location_distance(lua_State *L) {
const int args = lua_gettop(L);
if (args != 2) {
return luaL_argerror(L, args, "too many arguments");
}
lua_pushnumber(L, get_distance(*check_location(L, -2),
*check_location(L, -1)));
return 1;
}
static int location_project(lua_State *L) {
const int args = lua_gettop(L);
if (args != 3) {
return luaL_argerror(L, args, "too many arguments");
}
location_update(*check_location(L, -3),
luaL_checknumber(L, -2),
luaL_checknumber(L, -1));
lua_pop(L, 2);
return 1;
}
static int location_passed_point(lua_State *L) {
const int args = lua_gettop(L);
if (args != 3) {
return luaL_argerror(L, args, "too many arguments");
}
lua_pushboolean(L, location_passed_point(*check_location(L, -3),
*check_location(L, -2),
*check_location(L, -1)));
return 1;
}
static int location_path_proportion(lua_State *L) {
const int args = lua_gettop(L);
if (args != 3) {
return luaL_argerror(L, args, "too many arguments");
}
lua_pushnumber(L, location_path_proportion(*check_location(L, -3),
*check_location(L, -2),
*check_location(L, -1)));
return 1;
}
static int location_offset(lua_State *L) {
const int args = lua_gettop(L);
if (args != 3) {
return luaL_argerror(L, args, "too many arguments");
}
location_offset(*check_location(L, -3),
luaL_checknumber(L, -2),
luaL_checknumber(L, -1));
lua_pop(L, 2);
return 1;
}
static int location_equal(lua_State *L) {
const int args = lua_gettop(L);
if (args != 2) {
return luaL_argerror(L, args, "too many arguments");
}
Location *l2 = check_location(L, -2);
Location *l1 = check_location(L, -1);
lua_pushboolean(L, locations_are_same(*l1, *l2));
return 1;
}
static const luaL_Reg locLib[] = {
{"new", new_location},
{NULL, NULL}
};
static const luaL_Reg locationMeta[] = {
{"lat", location_lat},
{"long", location_long},
{"alt", location_alt},
{"relative_alt", location_relative_alt},
{"loiter_ccw", location_loiter_ccw},
{"terrain_alt", location_terrain_alt},
{"origin_alt", location_origin_alt},
{"loiter_xtrack", location_loiter_xtrack},
{"distance", location_distance},
{"project", location_project},
{"passed_point", location_passed_point},
{"path_proportion", location_path_proportion},
{"offset", location_offset},
{"__tostring", location_tostring},
{"__eq", location_equal},
{NULL, NULL}
};
static int ahrs_position(lua_State *L) {
check_arguments(L, 1, "ahrs:position");
new_location(L);
Location *loc = check_location(L, -1);
AP::ahrs().get_position(*loc);
return 1;
}
static int ahrs_get_home(lua_State *L) {
check_arguments(L, 1, "ahrs:home");
new_location(L);
Location *loc = check_location(L, -1);
*loc = AP::ahrs().get_home();
return 1;
}
static const luaL_Reg ahrsMeta[] = {
{"position", ahrs_position},
{"home", ahrs_get_home},
{NULL, NULL}
};
// all bindings
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");
// location metatable
luaL_newmetatable(L, "location");
luaL_setfuncs(L, locationMeta, 0);
lua_pushstring(L, "__index");
lua_pushvalue(L, -2);
lua_settable(L, -3);
lua_pop(L, 1);
luaL_newlib(L, locLib);
lua_setglobal(L, "loc");
// ahrs metatable
luaL_newmetatable(L, "ahrs");
luaL_setfuncs(L, ahrsMeta, 0);
lua_pushstring(L, "__index");
lua_pushvalue(L, -2);
lua_settable(L, -3);
lua_pop(L, 1);
// ahrs userdata
lua_newuserdata(L, 0); // lose the pointer, we don't really care about it
luaL_getmetatable(L, "ahrs");
lua_setmetatable(L, -2);
lua_setglobal(L, "ahrs");
}

1
libraries/AP_Scripting/lua_scripts.cpp
View File

@ -96,6 +96,7 @@ lua_scripts::script_info *lua_scripts::load_script(lua_State *L, char *filename)
lua_getglobal(L, "get_sandbox_env");
if (lua_pcall(L, 0, LUA_MULTRET, 0)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Scripting: Could not create sandbox: %s", lua_tostring(L, -1));
hal.console->printf("Lua: Scripting: Could not create sandbox: %s", lua_tostring(L, -1));
return nullptr;
}
lua_setupvalue(L, -2, 1);

2
libraries/AP_Scripting/scripts/sandbox.lua
View File

@ -31,5 +31,7 @@ function get_sandbox_env ()
-- ArduPilot specific
gcs = { send_text = gcs.send_text},
servo = { set_output_pwm = servo.set_output_pwm},
location = { new = loc.new},
ahrs = ahrs
}
end