diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 52e567350e..887dd9c6af 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -737,10 +737,12 @@ local AP_HAL__I2CDevice_ud = {} ---@param address integer function AP_HAL__I2CDevice_ud:set_address(address) end --- desc +-- 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 ----@return integer|nil -function AP_HAL__I2CDevice_ud:read_registers(register_num) end +---@param read_length? integer +---@return integer|table|nil +function AP_HAL__I2CDevice_ud:read_registers(register_num, read_length) end -- desc ---@param register_num integer diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 6c2ccdec01..0deb7d93b6 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -415,7 +415,7 @@ include AP_HAL/I2CDevice.h 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 0 UINT8_MAX uint8_t 0 UINT8_MAX -ap_object AP_HAL::I2CDevice method read_registers boolean uint8_t 0 UINT8_MAX &uint8_t'Null 1'literal +ap_object AP_HAL::I2CDevice manual read_registers AP_HAL__I2CDevice_read_registers ap_object AP_HAL::I2CDevice method set_address void uint8_t 0 UINT8_MAX diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 8eeedb92de..e564f6dfd4 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -320,6 +320,55 @@ int lua_get_i2c_device(lua_State *L) { return 1; } +int AP_HAL__I2CDevice_read_registers(lua_State *L) { + const int args = lua_gettop(L); + bool multi_register; + if (args == 2) { + multi_register = false; + } else if (args == 3) { + multi_register = true; + } else { + return luaL_argerror(L, args, "expected 1 or 2 arguments"); + } + + AP_HAL::I2CDevice * ud = *check_AP_HAL__I2CDevice(L, 1); + if (ud == NULL) { + return luaL_error(L, "Internal error, null pointer"); + } + + const lua_Integer raw_first_reg = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_first_reg >= MAX(0, 0)) && (raw_first_reg <= MIN(UINT8_MAX, UINT8_MAX))), 2, "argument out of range"); + const uint8_t first_reg = static_cast(raw_first_reg); + + uint8_t recv_length = 1; + if (multi_register) { + const lua_Integer raw_recv_length = luaL_checkinteger(L, 3); + luaL_argcheck(L, ((raw_recv_length >= MAX(0, 0)) && (raw_recv_length <= MIN(UINT8_MAX, UINT8_MAX))), 3, "argument out of range"); + recv_length = static_cast(raw_recv_length); + } + + uint8_t data[recv_length]; + + ud->get_semaphore()->take_blocking(); + const bool success = static_cast(ud->read_registers(first_reg, data, recv_length)); + ud->get_semaphore()->give(); + + if (success) { + if (!multi_register) { + lua_pushinteger(L, data[0]); + } else { + // push to table + lua_newtable(L); + for (uint8_t i=0; i < recv_length; i++) { + lua_pushinteger(L, i+1); + lua_pushinteger(L, data[i]); + lua_settable(L, -3); + } + } + } + return success; +} + #if HAL_MAX_CAN_PROTOCOL_DRIVERS int lua_get_CAN_device(lua_State *L) { diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index d50d7974b4..c32930e36c 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -7,5 +7,6 @@ int lua_micros(lua_State *L); 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 lua_get_CAN_device(lua_State *L); int lua_get_CAN_device2(lua_State *L);