AP_Scripting: use _chan to mark the methods that are mavlink channel based
This commit is contained in:
parent
dfe87732b5
commit
d056a33dca
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user