AP_Scripting: add support for scripting2 protocol

This commit is contained in:
Andrew Tridgell 2022-07-02 12:23:58 +10:00
parent 1b25babd8a
commit d8b98789e6
6 changed files with 55 additions and 14 deletions

View File

@ -73,6 +73,7 @@ public:
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
// Scripting CAN sensor
ScriptingCANSensor *_CAN_dev;
ScriptingCANSensor *_CAN_dev2;
#endif
// mission item buffer

View File

@ -25,8 +25,9 @@ class ScriptingCANBuffer;
class ScriptingCANSensor : public CANSensor {
public:
ScriptingCANSensor():CANSensor("Script") {
register_driver(AP_CANManager::Driver_Type::Driver_Type_Scripting);
ScriptingCANSensor(AP_CANManager::Driver_Type dtype)
: CANSensor("Script") {
register_driver(dtype);
}
// handler for outgoing frames, using uint32

View File

@ -1,19 +1,36 @@
-- This script is an example of reading from the CAN bus
-- Load CAN driver, using the scripting protocol and with a buffer size of 5
local driver = CAN.get_device(5)
-- Load CAN driver1. The first will attach to a protocol of 10, the 2nd to a protocol of 12
-- this allows the script to distinguish packets on two CAN interfaces
local driver1 = CAN.get_device(5)
local driver2 = CAN.get_device2(5)
if not driver1 and not driver2 then
gcs:send_text(0,"No scripting CAN interfaces found")
return
end
function show_frame(dnum, frame)
gcs:send_text(0,string.format("CAN[%u] msg from " .. tostring(frame:id()) .. ": %i, %i, %i, %i, %i, %i, %i, %i", dnum, frame:data(0), frame:data(1), frame:data(2), frame:data(3), frame:data(4), frame:data(5), frame:data(6), frame:data(7)))
end
function update()
-- Read a message from the buffer
frame = driver:read_frame()
-- see if we got any frames
if driver1 then
frame = driver1:read_frame()
if frame then
-- note that we have to be careful to keep the ID as a uint32_t userdata to retain precision
gcs:send_text(0,string.format("CAN msg from " .. tostring(frame:id()) .. ": %i, %i, %i, %i, %i, %i, %i, %i", frame:data(0), frame:data(1), frame:data(2), frame:data(3), frame:data(4), frame:data(5), frame:data(6), frame:data(7)))
show_frame(1, frame)
end
end
if driver2 then
frame = driver2:read_frame()
if frame then
show_frame(2, frame)
end
end
return update, 100
return update, 10
end

View File

@ -455,6 +455,7 @@ singleton AP_InertialSensor rename ins
singleton AP_InertialSensor method get_temperature float uint8_t 0 INS_MAX_INSTANCES
singleton CAN manual get_device lua_get_CAN_device
singleton CAN manual get_device2 lua_get_CAN_device2
singleton CAN depends HAL_MAX_CAN_PROTOCOL_DRIVERS
include AP_Scripting/AP_Scripting_CANSensor.h depends HAL_MAX_CAN_PROTOCOL_DRIVERS

View File

@ -338,7 +338,7 @@ int lua_get_CAN_device(lua_State *L) {
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::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");
}
@ -349,4 +349,25 @@ int lua_get_CAN_device(lua_State *L) {
return 1;
}
int lua_get_CAN_device2(lua_State *L) {
check_arguments(L, 1, "CAN:get_device2");
const uint32_t raw_buffer_len = coerce_to_uint32_t(L, 1);
luaL_argcheck(L, ((raw_buffer_len >= 1U) && (raw_buffer_len <= 25U)), 1, "argument out of range");
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

View File

@ -8,4 +8,4 @@ int lua_mission_receive(lua_State *L);
int AP_Logger_Write(lua_State *L);
int lua_get_i2c_device(lua_State *L);
int lua_get_CAN_device(lua_State *L);
int lua_get_CAN_device2(lua_State *L);