5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00

AP_Scripting: Add I2C transfer bindings

This adds bindings for an I2CDevice's transfer() function, an example,
and removes the nil return hint from the get_device() docs as it never
actually returns nil.
This commit is contained in:
Ian Burwell 2024-06-17 13:24:26 -04:00 committed by Andrew Tridgell
parent 0e7a5ef520
commit 9d34fa2a5e
5 changed files with 144 additions and 1 deletions
libraries/AP_Scripting

View File

@ -135,7 +135,7 @@ i2c = {}
---@param address integer -- device address 0 to 128
---@param clock? uint32_t_ud|integer|number -- optional bus clock, default 400000
---@param smbus? boolean -- optional sumbus flag, default false
---@return AP_HAL__I2CDevice_ud|nil
---@return AP_HAL__I2CDevice_ud
function i2c:get_device(bus, address, clock, smbus) end
-- EFI state structure
@ -1211,6 +1211,13 @@ local AP_HAL__I2CDevice_ud = {}
---@param address integer
function AP_HAL__I2CDevice_ud:set_address(address) end
-- Performs an I2C transfer, sending data_str bytes (see string.pack) and
-- returning a string of any requested read bytes (see string.unpack)
---@param data_str string
---@param read_length integer
---@return string|nil
function AP_HAL__I2CDevice_ud:transfer(data_str, read_length) end
-- If no read length is provided a single register will be read and returned.
-- If read length is provided a table of register values are returned.
---@param register_num integer

View File

@ -0,0 +1,107 @@
-- Runs the Built-In Self Test on the RM3100 LR circuits
-- Note COMPASS_DISBLMSK should have the 16th bit set to 1 (RM3100)
-- Init RM3100 on bus 0
local rm3100 = i2c:get_device(0, 0x20)
assert(rm3100 ~= nil, "i2c get_device error, cannot run RM3100 self test")
-- Queues a Built-In Self Test
function queue_test()
gcs:send_text(1, "Running RM3100 self test")
-- Queue a self test by setting BIST register
local ret = rm3100:transfer("\x33\x8F", 0)
if ret == nil then
gcs:send_text(1, "Rm3100 BIST transfer failed")
return queue_test, 1000
end
-- Send a POLL request to run a BIST
ret = rm3100:transfer("\x00\x70", 0)
if ret == nil then
gcs:send_text(1, "Rm3100 POLL transfer failed")
return queue_test, 1000
end
-- As a measurement takes time, delay a bit by scheduling a different function
return read_test, 1000
end
-- Reads back values from a Built-In Self Test
function read_test()
-- Read the BIST results
local results_str = rm3100:transfer("\x33", 1)
if results_str ~= nil then
local results = results_str:byte()
if results & (1 << 4) == 0 then
gcs:send_text(1, "RM3100 X is unhealthy")
else
gcs:send_text(1, "RM3100 X is OK")
end
if results & (1 << 5) == 0 then
gcs:send_text(1, "RM3100 Y is unhealthy")
else
gcs:send_text(1, "RM3100 Y is OK")
end
if results & (1 << 6) == 0 then
gcs:send_text(1, "RM3100 Z is unhealthy")
else
gcs:send_text(1, "RM3100 Z is OK")
end
else
gcs:send_text(1, "Rm3100 BIST read transfer failed")
return queue_test, 1000
end
-- Reset the BIST register
local ret = rm3100:transfer("\x33\x0F", 0)
if ret == nil then
gcs:send_text(1, "Rm3100 BIST reset transfer failed")
return queue_test, 1000
end
-- Send a POLL request to take a data point
ret = rm3100:transfer("\x00\x70", 0)
if ret == nil then
gcs:send_text(1, "Rm3100 POLL data transfer failed")
return queue_test, 1000
end
-- As a measurement takes time, delay a bit by scheduling a different function
return read_data, 1000
end
-- Reads data from the RM3100
function read_data()
-- Check that data is ready for a read
local status_str = rm3100:transfer("\x34", 1)
if status_str ~= nil then
local status = status_str:byte()
if status & (1 << 7) == 0 then
gcs:send_text(1, "RM3100 data not ready for reading")
return queue_test, 1000
end
else
gcs:send_text(1, "Rm3100 BIST status reg transfer failed")
return queue_test, 1000
end
-- Read measured values
local measurements_str = rm3100:transfer("\x24", 9)
if measurements_str ~= nil then
local MX, MY, MZ = string.unpack(">i3>i3>i3", measurements_str)
gcs:send_text(6, string.format("RM3100 Mag: X=%8d Y=%8d Z=%8d", MX, MY, MZ))
else
gcs:send_text(1, "Rm3100 data read transfer failed")
return queue_test, 1000
end
-- Loop back to the first function to run another set of tests
return queue_test, 1000
end
return queue_test, 1000

View File

@ -587,6 +587,7 @@ ap_object AP_HAL::I2CDevice semaphore-pointer
ap_object AP_HAL::I2CDevice method set_retries void uint8_t 0 20
ap_object AP_HAL::I2CDevice method write_register boolean uint8_t'skip_check uint8_t'skip_check
ap_object AP_HAL::I2CDevice manual read_registers AP_HAL__I2CDevice_read_registers 2 1
ap_object AP_HAL::I2CDevice manual transfer AP_HAL__I2CDevice_transfer 2 1
ap_object AP_HAL::I2CDevice method set_address void uint8_t'skip_check
include AP_HAL/utility/Socket.h depends (AP_NETWORKING_ENABLED==1)

View File

@ -656,6 +656,33 @@ int AP_HAL__I2CDevice_read_registers(lua_State *L) {
return success;
}
int AP_HAL__I2CDevice_transfer(lua_State *L) {
binding_argcheck(L, 3);
AP_HAL::I2CDevice * ud = *check_AP_HAL__I2CDevice(L, 1);
// Parse string of bytes to send
size_t send_len;
const uint8_t* send_data = (const uint8_t*)(lua_tolstring(L, 2, &send_len));
// Parse and setup rx buffer
uint32_t rx_len = get_uint8_t(L, 3);
uint8_t rx_data[rx_len];
// Transfer
ud->get_semaphore()->take_blocking();
const bool success = ud->transfer(send_data, send_len, rx_data, rx_len);
ud->get_semaphore()->give();
if (!success) {
return 0;
}
// Return a string
lua_pushlstring(L, (const char *)rx_data, rx_len);
return 1;
}
#if AP_SCRIPTING_CAN_SENSOR_ENABLED
int lua_get_CAN_device(lua_State *L) {

View File

@ -8,6 +8,7 @@ int lua_mission_receive(lua_State *L);
int AP_Logger_Write(lua_State *L);
int lua_get_i2c_device(lua_State *L);
int AP_HAL__I2CDevice_read_registers(lua_State *L);
int AP_HAL__I2CDevice_transfer(lua_State *L);
int lua_get_CAN_device(lua_State *L);
int lua_get_CAN_device2(lua_State *L);
int lua_serial_find_serial(lua_State *L);