AP_Scripting: use _chan to mark the methods that are mavlink channel based

This commit is contained in:
bugobliterator 2023-05-03 19:45:13 +10:00 committed by Andrew Tridgell
parent dfe87732b5
commit d056a33dca
6 changed files with 26 additions and 21 deletions

View File

@ -102,6 +102,7 @@ public:
struct mavlink_msg {
mavlink_message_t msg;
mavlink_channel_t chan;
uint32_t timestamp_ms;
};
static const int mavlink_input_queue_size = 5;

View File

@ -2863,21 +2863,23 @@ function remove(filename) end
mavlink = {}
-- initializes mavlink
--@param num_rx_msgid number of mavlink message types to receive
--@param msg_queue_length length of mavlink message queue
--@param num_rx_msgid number
--@param msg_queue_length
function mavlink:init(num_rx_msgid, msg_queue_length) end
-- marks mavlink message for receive, message id can be get using mavlink_msgs.get_msgid("MSG_NAME")
--@param msg_id mavlink message id
--@param msg_id number
function mavlink:receive_msgid(msg_id) end
-- receives mavlink message marked for receive using mavlink:receive_msgid
--@return mavlink message
function mavlink:receive() end
--@return mavlink_message bytes
--@return mavlink_channel number
--@return receive_timestamp number
function mavlink:receive_chan() end
-- sends mavlink message, to use this function the call should be like this:
-- mavlink:send(chan, mavlink_msgs.encode("MSG_NAME", {param1 = value1, param2 = value2, ...}})
--@param chan mavlink channel
--@param msgid mavlink message id
--@param message encoded message packet
function mavlink:send(chan, msgid, message) end
--@param mavlink_channel integer
--@param mavlink_message_id integer
--@param encoded_message_packet bytes
function mavlink:send_chan(chan, msgid, message) end

View File

@ -722,6 +722,6 @@ 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
singleton mavlink manual receive_msgid lua_mavlink_register_rx_msgid 1
singleton mavlink manual send_chan lua_mavlink_send_chan 3
singleton mavlink manual receive_chan lua_mavlink_receive_chan 0

View File

@ -65,7 +65,7 @@ int lua_mavlink_init(lua_State *L) {
return 0;
}
int lua_mavlink_receive(lua_State *L) {
int lua_mavlink_receive_chan(lua_State *L) {
binding_argcheck(L, 0);
struct AP_Scripting::mavlink_msg msg;
@ -81,14 +81,15 @@ int lua_mavlink_receive(lua_State *L) {
luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg));
luaL_pushresult(&b);
lua_pushinteger(L, msg.chan);
return 2;
lua_pushinteger(L, msg.timestamp_ms);
return 3;
} else {
// no MAVLink to handle, just return no results
return 0;
}
}
int lua_mavlink_receive_msgid(lua_State *L) {
int lua_mavlink_register_rx_msgid(lua_State *L) {
binding_argcheck(L, 1);
const int msgid = luaL_checkinteger(L, -1);
@ -124,7 +125,7 @@ int lua_mavlink_receive_msgid(lua_State *L) {
return 1;
}
int lua_mavlink_send(lua_State *L) {
int lua_mavlink_send_chan(lua_State *L) {
binding_argcheck(L, 3);
const mavlink_channel_t chan = (mavlink_channel_t)luaL_checkinteger(L, 1);

View File

@ -15,6 +15,6 @@ 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);
int lua_mavlink_receive_chan(lua_State *L);
int lua_mavlink_register_rx_msgid(lua_State *L);
int lua_mavlink_send_chan(lua_State *L);

View File

@ -19,8 +19,9 @@ function str_to_bytes(str)
return bytes
end
function update()
local msg = mavlink.receive()
local msg,chan,timestamp_ms = mavlink.receive_chan()
if msg then
gcs:send_text(6, string.format("Received message on channel %d at %d", chan, timestamp_ms))
local 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))
@ -30,7 +31,7 @@ function update()
end
test_named_value = test_named_value + 1.0
-- send named value float
mavlink.send(0, mavlink_msgs.encode("NAMED_VALUE_FLOAT", {time_boot_ms = millis():toint(), name = str_to_bytes("test"), value = test_named_value}))
mavlink.send_chan(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