Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Scripting/lua_bindings.cpp

463 lines
15 KiB
C++
Raw Blame History

#include <AP_Common/AP_Common.h>
#include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h>
#include "lua_bindings.h"
#include "lua_boxed_numerics.h"
#include <AP_Scripting/lua_generated_bindings.h>
#include <AP_Scripting/AP_Scripting.h>
#include <string.h>
extern const AP_HAL::HAL& hal;
// millis
int lua_millis(lua_State *L) {
binding_argcheck(L, 0);
new_uint32_t(L);
*check_uint32_t(L, -1) = AP_HAL::millis();
return 1;
}
// micros
int lua_micros(lua_State *L) {
binding_argcheck(L, 0);
new_uint32_t(L);
*check_uint32_t(L, -1) = AP_HAL::micros();
return 1;
}
int lua_mission_receive(lua_State *L) {
binding_argcheck(L, 0);
ObjectBuffer<struct AP_Scripting::scripting_mission_cmd> *input = AP::scripting()->mission_data;
if (input == nullptr) {
// no mission items ever received
return 0;
}
struct AP_Scripting::scripting_mission_cmd cmd;
if (!input->pop(cmd)) {
// no new item
return 0;
}
new_uint32_t(L);
*check_uint32_t(L, -1) = cmd.time_ms;
lua_pushinteger(L, cmd.p1);
lua_pushnumber(L, cmd.content_p1);
lua_pushnumber(L, cmd.content_p2);
lua_pushnumber(L, cmd.content_p3);
return 5;
}
int AP_Logger_Write(lua_State *L) {
AP_Logger * AP_logger = AP_Logger::get_singleton();
if (AP_logger == nullptr) {
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
const int args = lua_gettop(L) - arg_offset;
if (args < 4) {
return luaL_argerror(L, args, "too few arguments");
}
const char * name = luaL_checkstring(L, 1 + arg_offset);
const char * labels = luaL_checkstring(L, 2 + arg_offset);
const char * fmt = luaL_checkstring(L, 3 + arg_offset);
// cheack the name, labels and format are not too long
if (strlen(name) >= LS_NAME_SIZE) {
return luaL_error(L, "Name must be 4 or less chars long");
}
uint8_t length = strlen(labels);
if (length >= (LS_LABELS_SIZE - 7)) { // need 7 chars to add 'TimeUS,'
return luaL_error(L, "labels must be less than 58 chars long");
}
// Count the number of commas
uint8_t commas = 1;
for (uint8_t i=0; i<length; i++) {
if (labels[i] == ',') {
commas++;
}
}
length = strlen(fmt);
if (length >= (LS_FORMAT_SIZE - 1)) { // need 1 char to add timestamp
return luaL_error(L, "format must be less than 15 chars long");
}
// check the number of arguments matches the number of values in the label
if (length != commas) {
return luaL_argerror(L, args, "label does not match format");
}
bool have_units = false;
if (args - 5 == length) {
// check if there are enough arguments for units and multiplyers
have_units = true;
} else if (args - 3 != length) {
// check the number of arguments matches the length of the foramt string
return luaL_argerror(L, args, "format does not match No. of arguments");
}
// prepend timestamp to format and labels
char label_cat[LS_LABELS_SIZE];
strcpy(label_cat,"TimeUS,");
strcat(label_cat,labels);
char fmt_cat[LS_FORMAT_SIZE];
strcpy(fmt_cat,"Q");
strcat(fmt_cat,fmt);
// Need to declare these here so they don't go out of scope
char units_cat[LS_FORMAT_SIZE];
char multipliers_cat[LS_FORMAT_SIZE];
uint8_t field_start = 4;
struct AP_Logger::log_write_fmt *f;
if (!have_units) {
// ask for a mesage type
f = AP_logger->msg_fmt_for_name(name, label_cat, nullptr, nullptr, fmt_cat, true, true);
} else {
// read in units and multiplers strings
field_start += 2;
const char * units = luaL_checkstring(L, 4 + arg_offset);
const char * multipliers = luaL_checkstring(L, 5 + arg_offset);
if (length != strlen(units)) {
return luaL_error(L, "units must be same length as format");
}
if (length != strlen(multipliers)) {
return luaL_error(L, "multipliers must be same length as format");
}
// prepend timestamp to units and multiplyers
strcpy(units_cat,"s");
strcat(units_cat,units);
strcpy(multipliers_cat,"F");
strcat(multipliers_cat,multipliers);
// ask for a mesage type
f = AP_logger->msg_fmt_for_name(name, label_cat, units_cat, multipliers_cat, fmt_cat, true, true);
}
if (f == nullptr) {
// unable to map name to a messagetype; could be out of
// msgtypes, could be out of slots, ...
return luaL_argerror(L, args, "could not map message type");
}
// work out how long the block will be
int16_t msg_len = AP_logger->Write_calc_msg_len(fmt_cat);
if (msg_len == -1) {
return luaL_argerror(L, args, "unknown format");
}
lua_Alloc allocf = lua_getallocf(L, nullptr);
char *buffer = (char*)allocf(nullptr, nullptr, 0, msg_len);
if (buffer == nullptr) {
return luaL_error(L, "Buffer allocation failed");
}
// add logging headers
uint8_t offset = 0;
buffer[offset++] = HEAD_BYTE1;
buffer[offset++] = HEAD_BYTE2;
buffer[offset++] = f->msg_type;
// timestamp is always first value
const uint64_t now = AP_HAL::micros64();
memcpy(&buffer[offset], &now, sizeof(uint64_t));
offset += sizeof(uint64_t);
for (uint8_t i=field_start; i<=args; i++) {
uint8_t charlen = 0;
uint8_t index = have_units ? i-5 : i-3;
uint8_t arg_index = i + arg_offset;
switch(fmt_cat[index]) {
// logger varable types not available to scripting
// 'b': int8_t
// 'h': int16_t
// 'c': int16_t
// 'd': double
// 'H': uint16_t
// 'C': uint16_t
// 'Q': uint64_t
// 'q': int64_t
// 'a': arrays
case 'i':
case 'L':
case 'e': {
int isnum;
const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum);
if (!isnum) {
allocf(nullptr, buffer, 0, 0);
luaL_argerror(L, arg_index, "argument out of range");
// no return
}
const int32_t tmp = tmp1;
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
offset += sizeof(int32_t);
break;
}
case 'f': {
int isnum;
const lua_Number tmp1 = lua_tonumberx(L, arg_index, &isnum);
if (!isnum) {
allocf(nullptr, buffer, 0, 0);
luaL_argerror(L, arg_index, "argument out of range");
// no return
}
const float tmp = tmp1;
memcpy(&buffer[offset], &tmp, sizeof(float));
offset += sizeof(float);
break;
}
case 'n': {
charlen = 4;
break;
}
case 'M':
case 'B': {
int isnum;
const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum);
if (!isnum || (tmp1 < 0) || (tmp1 > UINT8_MAX)) {
allocf(nullptr, buffer, 0, 0);
luaL_argerror(L, arg_index, "argument out of range");
// no return
}
uint8_t tmp = static_cast<uint8_t>(tmp1);
memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
offset += sizeof(uint8_t);
break;
}
case 'I':
case 'E': {
uint32_t tmp;
void * ud = luaL_testudata(L, arg_index, "uint32_t");
if (ud != nullptr) {
tmp = *static_cast<uint32_t *>(ud);
} else {
int success;
const lua_Integer v_int = lua_tointegerx(L, arg_index, &success);
if (success) {
tmp = v_int;
} else {
const lua_Number v_float = lua_tonumberx(L, arg_index, &success);
if (!success || (v_float < 0) || (v_float > float(UINT32_MAX))) {
allocf(nullptr, buffer, 0, 0);
luaL_argerror(L, arg_index, "argument out of range");
// no return
}
tmp = v_float;
}
}
memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
offset += sizeof(uint32_t);
break;
}
case 'N': {
charlen = 16;
break;
}
case 'Z': {
charlen = 64;
break;
}
default: {
allocf(nullptr, buffer, 0, 0);
luaL_error(L, "%c unsupported format",fmt_cat[index]);
// no return
}
}
if (charlen != 0) {
size_t slen;
const char *tmp = lua_tolstring(L, arg_index, &slen);
if (tmp == nullptr) {
allocf(nullptr, buffer, 0, 0);
luaL_argerror(L, arg_index, "argument out of range");
// no return
}
if (slen > charlen) {
allocf(nullptr, buffer, 0, 0);
luaL_error(L, "arg %d too long for %c format",arg_index,fmt_cat[index]);
// no return
}
memcpy(&buffer[offset], tmp, slen);
memset(&buffer[offset+slen], 0, charlen-slen);
offset += charlen;
}
}
AP_logger->Safe_Write_Emit_FMT(f);
AP_logger->WriteBlock(buffer,msg_len);
allocf(nullptr, buffer, 0, 0);
return 0;
}
int lua_get_i2c_device(lua_State *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) {
return luaL_argerror(L, args, "require i2c bus and address");
}
if (args > 4) {
return luaL_argerror(L, args, "too many arguments");
}
const lua_Integer bus_in = get_integer(L, 1 + arg_offset, 0, 4);
const uint8_t bus = static_cast<uint8_t>(bus_in);
const lua_Integer address_in = get_integer(L, 2 + arg_offset, 0, 128);
const uint8_t address = static_cast<uint8_t>(address_in);
// optional arguments, use the same defaults as the hal get_device function
uint32_t bus_clock = 400000;
bool use_smbus = false;
if (args > 2) {
bus_clock = coerce_to_uint32_t(L, 3 + arg_offset);
if (args > 3) {
use_smbus = static_cast<bool>(lua_toboolean(L, 4 + arg_offset));
}
}
static_assert(SCRIPTING_MAX_NUM_I2C_DEVICE >= 0, "There cannot be a negative number of I2C devices");
if (AP::scripting()->num_i2c_devices >= SCRIPTING_MAX_NUM_I2C_DEVICE) {
return luaL_argerror(L, 1, "no i2c devices available");
}
AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices] = new AP_HAL::OwnPtr<AP_HAL::I2CDevice>;
if (AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices] == nullptr) {
return luaL_argerror(L, 1, "i2c device nullptr");
}
*AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices] = std::move(hal.i2c_mgr->get_device(bus, address, bus_clock, use_smbus));
if (AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices] == nullptr || AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices]->get() == nullptr) {
return luaL_argerror(L, 1, "i2c device nullptr");
}
new_AP_HAL__I2CDevice(L);
*check_AP_HAL__I2CDevice(L, -1) = AP::scripting()->_i2c_dev[AP::scripting()->num_i2c_devices]->get();
AP::scripting()->num_i2c_devices++;
return 1;
}
int AP_HAL__I2CDevice_read_registers(lua_State *L) {
const int args = lua_gettop(L);
bool multi_register;
if (args == 2) {
multi_register = false;
} else if (args == 3) {
multi_register = true;
} else {
return luaL_argerror(L, args, "expected 1 or 2 arguments");
}
AP_HAL::I2CDevice * ud = *check_AP_HAL__I2CDevice(L, 1);
if (ud == NULL) {
return luaL_error(L, "Internal error, null pointer");
}
const uint8_t first_reg = get_uint8_t(L, 2);
uint8_t recv_length = 1;
if (multi_register) {
recv_length = get_uint8_t(L, 3);
}
uint8_t data[recv_length];
ud->get_semaphore()->take_blocking();
const bool success = static_cast<bool>(ud->read_registers(first_reg, data, recv_length));
ud->get_semaphore()->give();
if (success) {
if (!multi_register) {
lua_pushinteger(L, data[0]);
} else {
// push to table
lua_newtable(L);
for (uint8_t i=0; i < recv_length; i++) {
lua_pushinteger(L, i+1);
lua_pushinteger(L, data[i]);
lua_settable(L, -3);
}
}
}
return success;
}
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
int lua_get_CAN_device(lua_State *L) {
// Allow : and . access
const int arg_offset = (luaL_testudata(L, 1, "CAN") != NULL) ? 1 : 0;
binding_argcheck(L, 1 + arg_offset);
const uint32_t raw_buffer_len = get_uint32(L, 1 + arg_offset, 1, 25);
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
if (AP::scripting()->_CAN_dev == nullptr) {
AP::scripting()->_CAN_dev = new ScriptingCANSensor(AP_CANManager::Driver_Type::Driver_Type_Scripting);
if (AP::scripting()->_CAN_dev == nullptr) {
return luaL_argerror(L, 1, "CAN device nullptr");
}
}
new_ScriptingCANBuffer(L);
*check_ScriptingCANBuffer(L, -1) = AP::scripting()->_CAN_dev->add_buffer(buffer_len);
return 1;
}
int lua_get_CAN_device2(lua_State *L) {
// Allow : and . access
const int arg_offset = (luaL_testudata(L, 1, "CAN") != NULL) ? 1 : 0;
binding_argcheck(L, 1 + arg_offset);
const uint32_t raw_buffer_len = get_uint32(L, 1 + arg_offset, 1, 25);
const uint32_t buffer_len = static_cast<uint32_t>(raw_buffer_len);
if (AP::scripting()->_CAN_dev2 == nullptr) {
AP::scripting()->_CAN_dev2 = new ScriptingCANSensor(AP_CANManager::Driver_Type::Driver_Type_Scripting2);
if (AP::scripting()->_CAN_dev2 == nullptr) {
return luaL_argerror(L, 1, "CAN device nullptr");
}
}
new_ScriptingCANBuffer(L);
*check_ScriptingCANBuffer(L, -1) = AP::scripting()->_CAN_dev2->add_buffer(buffer_len);
return 1;
}
#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
Reference in New Issue View Git Blame Copy Permalink