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

AP_Scripting: add mavlink support to scripting

This commit is contained in:
bugobliterator 2023-03-10 09:45:30 +11:00 committed by Andrew Tridgell
parent a934c90e69
commit 076d00fd1e
6 changed files with 100 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Scripting/AP_Scripting.cpp
View File

@ -140,8 +140,6 @@ AP_Scripting::AP_Scripting() {
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
_singleton = this;
memset(mavlink_data.accept_msg_ids, -1, sizeof(mavlink_data.accept_msg_ids));
}
void AP_Scripting::init(void) {
@ -341,7 +339,7 @@ void AP_Scripting::restart_all()
}
void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan) {
if (mavlink_data.input == nullptr) {
if (mavlink_data.rx_buffer == nullptr) {
return;
}
@ -349,45 +347,17 @@ void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_ch
WITH_SEMAPHORE(mavlink_data.sem);
for (int32_t id : mavlink_data.accept_msg_ids) {
if (id == -1) {
for (uint16_t i = 0; i < mavlink_data.accept_msg_ids_size; i++) {
if (mavlink_data.accept_msg_ids[i] == -1) {
return;
}
if (id == msg.msgid) {
mavlink_data.input->push(data);
if (mavlink_data.accept_msg_ids[i] == msg.msgid) {
mavlink_data.rx_buffer->push(data);
return;
}
}
}
void AP_Scripting::send_message(const mavlink_channel_t chan) {
if (mavlink_data.output == nullptr) {
return;
}
struct mavlink_output data;
if (mavlink_data.output->peek(data) && (data.chan == chan)) {
// FIXME: The data thats in this mavlink_msg_entry_t should be provided from the script, which allows
// sending entirely new messages as outputs. At the moment we can only encode messages that
// are known at compile time. This is fine as a starting point as this is symmetrical to the
// decoding side of the scripting support
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(data.msgid);
if (entry == nullptr) {
return;
}
if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) {
_mav_finalize_message_chan_send(chan,
entry->msgid,
data.data,
entry->min_msg_len,
entry->max_msg_len,
entry->crc_extra);
UNUSED_RESULT(mavlink_data.output->pop(data));
}
}
}
AP_Scripting *AP_Scripting::_singleton = nullptr;
namespace AP {

14
libraries/AP_Scripting/AP_Scripting.h
View File

@ -45,8 +45,6 @@ public:
void handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan);
void send_message(const mavlink_channel_t chan);
static AP_Scripting * get_singleton(void) { return _singleton; }
static const struct AP_Param::GroupInfo var_info[];
@ -106,19 +104,13 @@ public:
mavlink_channel_t chan;
};
struct mavlink_output {
char data[256]; // maximum payload size
int32_t msgid;
mavlink_channel_t chan;
};
static const int mavlink_input_queue_size = 5;
static const int mavlink_output_queue_size = 3;
struct mavlink {
ObjectBuffer<struct mavlink_msg> * input;
ObjectBuffer<struct mavlink_output> * output;
int32_t accept_msg_ids[16];
ObjectBuffer<struct mavlink_msg> *rx_buffer;
int *accept_msg_ids;
uint16_t accept_msg_ids_size;
HAL_Semaphore sem;
} mavlink_data;

153
libraries/AP_Scripting/examples/mavlink.lua
View File

@ -1,134 +1,37 @@
function decode_header(message)
-- build up a map of the result
local result = {}
mavlink_msgs = require("mavlink_msgs")
local read_marker = 3
msg_map = {}
-- id the MAVLink version
result.protocol_version, read_marker = string.unpack("<B", message, read_marker)
if (result.protocol_version == 0xFE) then -- mavlink 1
result.protocol_version = 1
elseif (result.protocol_version == 0XFD) then --mavlink 2
result.protocol_version = 2
else
error("Invalid magic byte")
end
heartbeat_msgid = mavlink_msgs.get_msgid("HEARTBEAT")
local payload_len, read_marker = string.unpack("<B", message, read_marker) -- payload is always the second byte
-- strip the incompat/compat flags
result.incompat_flags, result.compat_flags, read_marker = string.unpack("<BB", message, read_marker)
-- fetch seq/sysid/compid
result.seq, result.sysid, result.compid, read_marker = string.unpack("<BBB", message, read_marker)
-- fetch the message id
result.msgid, read_marker = string.unpack("<I3", message, read_marker)
return result, read_marker
end
function decode(message, messages)
local result, offset = decode_header(message)
local message_map = messages[result.msgid]
if not message_map then
-- we don't know how to decode this message, bail on it
return nil
end
-- map all the fields out
for i,v in ipairs(message_map) do
if v[3] then
result[v[1]] = {}
for j=1,v[3] do
result[v[1]][j], offset = string.unpack(v[2], message, offset)
end
else
result[v[1]], offset = string.unpack(v[2], message, offset)
msg_map[heartbeat_msgid] = "HEARTBEAT"
-- initialise mavlink rx with number of messages, and buffer depth
mavlink.init(1, 10)
-- register message id to receive
mavlink.receive_msgid(heartbeat_msgid)
test_named_value = 0.0
function str_to_bytes(str)
str_len = string.len(str)
bytes = {}
for i = 1, str_len do
bytes[i] = string.byte(str, i)
end
end
-- ignore the idea of a checksum
return result;
return bytes
end
function encode(msgid, message, messages)
local message_map = messages[msgid]
if not message_map then
-- we don't know how to encode this message, bail on it
error("Unknown MAVLink message " .. msgid)
end
local packString = "<"
local packedTable = {}
local packedIndex = 1
for i,v in ipairs(message_map) do
if v[3] then
packString = (packString .. string.rep(string.sub(v[2], 2), v[3]))
for j = 1, v[3] do
packedTable[packedIndex] = message[message_map[i][1]][j]
packedIndex = packedIndex + 1
end
else
packString = (packString .. string.sub(v[2], 2))
packedTable[packedIndex] = message[message_map[i][1]]
packedIndex = packedIndex + 1
end
end
return string.pack(packString, table.unpack(packedTable))
end
messages = {}
messages[76] = { -- COMMAND_LONG
{ "param1", "<f" },
{ "param2", "<f" },
{ "param3", "<f" },
{ "param4", "<f" },
{ "param5", "<f" },
{ "param6", "<f" },
{ "param7", "<f" },
{ "command", "<I2" },
{ "target_system", "<B" },
{ "target_component", "<B" },
{ "confirmation", "<B" },
}
messages[194] = { -- PID_TUNING
{ "desired", "<f" },
{ "achieved", "<f" },
{ "FF", "<f" },
{ "P", "<f" },
{ "I", "<f" },
{ "D", "<f" },
{ "axis", "<B" },
}
bar = {}
bar.desired = 12.5
bar.achieved = 13.5
bar.FF = 14.5
bar.P = 15.5
bar.I = 16.5
bar.D = 17.5
bar.axis = 6
mavlink.register_msgid(76)
function update()
local message = mavlink.receive()
local decoded_msg
if message then
decoded_msg = decode(message, messages)
gcs:send_text(0, string.format("%d", decoded_msg.command))
end
bar.desired = math.random()
mavlink.send(0, 194, encode(194, bar, messages))
return update, 500
local msg = mavlink.receive()
if msg then
parsed_msg = mavlink_msgs.decode(msg, msg_map)
if parsed_msg.msgid == heartbeat_msgid then
gcs:send_text(6, string.format("Received heartbeat from %d", parsed_msg.sysid))
end
else
gcs:send_text(6, "No heartbeat received")
end
test_named_value = test_named_value + 1.0
-- send named value float to channel 0
mavlink.send(0, mavlink_msgs.encode("NAMED_VALUE_FLOAT", {time_boot_ms = millis():toint(), name = str_to_bytes("test"), value = test_named_value}))
return update, 1000
end
return update()

5
libraries/AP_Scripting/generator/description/bindings.desc
View File

@ -720,3 +720,8 @@ userdata uint32_t manual tofloat uint32_t_tofloat 0
global manual dirlist lua_dirlist 1
global manual remove lua_removefile 1
singleton mavlink manual init lua_mavlink_init 2
singleton mavlink manual receive_msgid lua_mavlink_receive_msgid 1
singleton mavlink manual send lua_mavlink_send 3
singleton mavlink manual receive lua_mavlink_receive 0

111
libraries/AP_Scripting/lua_bindings.cpp
View File

@ -38,17 +38,44 @@ int lua_micros(lua_State *L) {
return 1;
}
static int lua_mavlink_receive(lua_State *L) {
check_arguments(L, 0, "receive");
int lua_mavlink_init(lua_State *L) {
binding_argcheck(L, 2);
WITH_SEMAPHORE(AP::scripting()->mavlink_data.sem);
// get the depth of receive queue
const int queue_size = luaL_checkinteger(L, -1);
// get number of msgs to accept
const int num_msgs = luaL_checkinteger(L, -2);
struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data;
if (data.rx_buffer == nullptr) {
data.rx_buffer = new ObjectBuffer<struct AP_Scripting::mavlink_msg>(queue_size);
if (data.rx_buffer == nullptr) {
return luaL_error(L, "Failed to allocate mavlink rx buffer");
}
}
if (data.accept_msg_ids == nullptr) {
data.accept_msg_ids = new int[num_msgs];
if (data.accept_msg_ids == nullptr) {
return luaL_error(L, "Failed to allocate mavlink rx registry");
}
data.accept_msg_ids_size = num_msgs;
memset(data.accept_msg_ids, -1, sizeof(int) * num_msgs);
}
return 0;
}
int lua_mavlink_receive(lua_State *L) {
binding_argcheck(L, 0);
struct AP_Scripting::mavlink_msg msg;
ObjectBuffer<struct AP_Scripting::mavlink_msg> *input = AP::scripting()->mavlink_data.input;
ObjectBuffer<struct AP_Scripting::mavlink_msg> *rx_buffer = AP::scripting()->mavlink_data.rx_buffer;
if (input == nullptr) {
luaL_error(L, "Never subscribed to a message");
if (rx_buffer == nullptr) {
return luaL_error(L, "Never subscribed to a message");
}
if (input->pop(msg)) {
if (rx_buffer->pop(msg)) {
luaL_Buffer b;
luaL_buffinit(L, &b);
luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg));
@ -61,41 +88,30 @@ static int lua_mavlink_receive(lua_State *L) {
}
}
static int lua_mavlink_register_msgid(lua_State *L) {
check_arguments(L, 1, "register_msgid");
luaL_checkstack(L, 1, "Out of stack");
int lua_mavlink_receive_msgid(lua_State *L) {
binding_argcheck(L, 1);
const int msgid = luaL_checkinteger(L, -1);
luaL_argcheck(L, ((msgid >= 0) && (msgid < (1 << 24))), 1, "msgid out of range");
struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data;
if (data.input == nullptr) {
{ // WITH_SEMAPHORE cannot be used in the same scope as a luaL_error (or any thing else that jumps away)
WITH_SEMAPHORE(data.sem);
data.input = new ObjectBuffer<struct AP_Scripting::mavlink_msg>(AP_Scripting::mavlink_input_queue_size);
}
if (data.input == nullptr) {
return luaL_error(L, "Unable to allocate MAVLink buffer");
}
}
// check that we aren't currently watching this ID
for (int id : data.accept_msg_ids) {
if (id == msgid) {
for (uint8_t i = 0; i < data.accept_msg_ids_size; i++) {
if (data.accept_msg_ids[i] == msgid) {
lua_pushboolean(L, false);
return 1;
}
}
int i = 0;
for (i = 0; i < (int)ARRAY_SIZE(data.accept_msg_ids); i++) {
for (i = 0; i < data.accept_msg_ids_size; i++) {
if (data.accept_msg_ids[i] == -1) {
break;
}
}
if (i >= (int)ARRAY_SIZE(data.accept_msg_ids)) {
if (i >= data.accept_msg_ids_size) {
return luaL_error(L, "Out of MAVLink ID's to monitor");
}
@ -108,8 +124,8 @@ static int lua_mavlink_register_msgid(lua_State *L) {
return 1;
}
static int lua_mavlink_send(lua_State *L) {
check_arguments(L, 3, "send");
int lua_mavlink_send(lua_State *L) {
binding_argcheck(L, 3);
const mavlink_channel_t chan = (mavlink_channel_t)luaL_checkinteger(L, 1);
luaL_argcheck(L, (chan < MAVLINK_COMM_NUM_BUFFERS), 1, "channel out of range");
@ -119,20 +135,22 @@ static int lua_mavlink_send(lua_State *L) {
const char *packet = luaL_checkstring(L, 3);
struct AP_Scripting::mavlink &mavlink_data = AP::scripting()->mavlink_data;
if (mavlink_data.output == nullptr) {
mavlink_data.output = new ObjectBuffer<struct AP_Scripting::mavlink_output>(AP_Scripting::mavlink_output_queue_size);
if (mavlink_data.output == nullptr) {
return luaL_error(L, "Unable to allocate MAVLink output queue");
}
// FIXME: The data that's in this mavlink_msg_entry_t should be provided from the script, which allows
// sending entirely new messages as outputs. At the moment we can only encode messages that
// are known at compile time. This is fine as a starting point as this is symmetrical to the
// decoding side of the scripting support
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
if (entry == nullptr) {
return luaL_error(L, "Unknown MAVLink message ID (%d)", msgid);
}
if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) {
_mav_finalize_message_chan_send(chan,
entry->msgid,
packet,
entry->min_msg_len,
entry->max_msg_len,
entry->crc_extra);
AP_Scripting::mavlink_output data {};
memcpy(data.data, packet, MIN(sizeof(data.data), lua_rawlen(L, 3)));
data.chan = chan;
data.msgid = msgid;
if (mavlink_data.output->push(data)) {
lua_pushboolean(L, true);
} else {
lua_pushboolean(L, false);
@ -511,25 +529,6 @@ int lua_get_i2c_device(lua_State *L) {
return 1;
}
static const luaL_Reg mavlink_functions[] =
{
{"receive", lua_mavlink_receive},
{"register_msgid", lua_mavlink_register_msgid},
{"send", lua_mavlink_send},
{NULL, NULL}
};
void load_lua_bindings(lua_State *L) {
lua_pushstring(L, "logger");
luaL_newlib(L, AP_Logger_functions);
lua_settable(L, -3);
lua_pushstring(L, "mavlink");
luaL_newlib(L, mavlink_functions);
lua_settable(L, -3);
luaL_setfuncs(L, global_functions, 0);
}
int AP_HAL__I2CDevice_read_registers(lua_State *L) {
const int args = lua_gettop(L);

4
libraries/AP_Scripting/lua_bindings.h
View File

@ -14,3 +14,7 @@ int lua_dirlist(lua_State *L);
int lua_removefile(lua_State *L);
int SRV_Channels_get_safety_state(lua_State *L);
int lua_get_PWMSource(lua_State *L);
int lua_mavlink_init(lua_State *L);
int lua_mavlink_receive(lua_State *L);
int lua_mavlink_receive_msgid(lua_State *L);
int lua_mavlink_send(lua_State *L);