From 67cc35ba217b2f7a508bd0e085777933af2c86ca Mon Sep 17 00:00:00 2001 From: Ian Burwell Date: Tue, 16 Jul 2024 15:58:06 -0400 Subject: [PATCH] AP_Scripting: Add BQ40Z shutdown example This example intercepts PREFLIGHT_REBOOT_SHUTDOWN COMMAND_LONG's and if param1==2, it shuts down the BQ40Z smart battery BMS. Otherwise it passes through the COMMAND_LONG as a COMMAND_INT (this required updating the gcs:run_command_int to return a MAV_RESULT rather than a bool). --- libraries/AP_Scripting/docs/docs.lua | 2 +- .../examples/BQ40Z_bms_shutdown.lua | 108 ++++++++++++++++++ libraries/AP_Scripting/lua_bindings.cpp | 6 +- 3 files changed, 111 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_Scripting/examples/BQ40Z_bms_shutdown.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 4dda3080767d8..54d45f068612d 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2615,7 +2615,7 @@ function gcs:last_seen() end -- call a MAVLink MAV_CMD_xxx command via command_int interface ---@param command integer -- MAV_CMD_xxx ---@param params table -- parameters of p1, p2, p3, p4, x, y and z and frame. Any not specified taken as zero ----@return boolean +---@return integer -- MAV_RESULT function gcs:run_command_int(command, params) end -- The relay library provides access to controlling relay outputs. diff --git a/libraries/AP_Scripting/examples/BQ40Z_bms_shutdown.lua b/libraries/AP_Scripting/examples/BQ40Z_bms_shutdown.lua new file mode 100644 index 0000000000000..27dee597cc91b --- /dev/null +++ b/libraries/AP_Scripting/examples/BQ40Z_bms_shutdown.lua @@ -0,0 +1,108 @@ +-- TI BQ40Z BMS shutdown script + +local mavlink_msgs = require("MAVLink/mavlink_msgs") + +local COMMAND_ACK_ID = mavlink_msgs.get_msgid("COMMAND_ACK") +local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG") +local msg_map = {} +msg_map[COMMAND_ACK_ID] = "COMMAND_ACK" +msg_map[COMMAND_LONG_ID] = "COMMAND_LONG" + +local PARAM_TABLE_KEY = 51 +local PARAM_TABLE_PREFIX = "BATT_BQ40Z_" +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table') +--[[ + // @Param: BATT_BQ40Z_BUS + // @DisplayName: Bus number for the BQ40Z + // @Description: Bus number for the BQ40Z + // @Range: 0 3 + // @User: Standard +--]] +local BATT_BQ40Z_BUS = bind_add_param('BUS', 1, 0) + + +local MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 +local MAV_RESULT_ACCEPTED = 0 +local MAV_RESULT_FAILED = 4 + +-- Initialize MAVLink rx with number of messages, and buffer depth +mavlink:init(1, 10) +-- Register message id to receive +mavlink:register_rx_msgid(COMMAND_LONG_ID) +-- Block MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN so we can handle the ACK +mavlink:block_command(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) + +-- Init BMS i2c device +local i2c_addr = BATT_BQ40Z_BUS:get() +assert(i2c_addr ~= nil, "BATT_BQ40Z_BUS not retrievable") +local bms = i2c:get_device(math.floor(i2c_addr), 0x0B) + +-- Exit emergency shutdown (for BQ40Z60, twice for redundancy) +bms:transfer("\x00\xA7\x23", 0) +bms:transfer("\x00\xA7\x23", 0) + +-- Function that is returned to the AP scheduler when we want to shutdown +local function shutdown_loop() + local ret = bms:transfer("\x00\x10\x00", 0) + if ret == nil then + gcs:send_text(0, "BQ40Z shutdown transfer failed") + end + + return shutdown_loop, 500 +end + +-- Main loop +local function update() + local msg, chan = mavlink:receive_chan() + local parsed_msg = nil + + if (msg ~= nil) then + parsed_msg = mavlink_msgs.decode(msg, msg_map) + end + + if parsed_msg ~= nil and (parsed_msg.msgid == COMMAND_LONG_ID) and (parsed_msg.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) then + -- Prepare ack + local ack = {} + ack.command = parsed_msg.command + ack.result = MAV_RESULT_ACCEPTED + ack.progress = 0 + ack.result_param2 = 0 + ack.target_system = parsed_msg.sysid + ack.target_component = parsed_msg.compid + + -- Don't shutdown if armed + if arming:is_armed() and parsed_msg.param1 == 2 then + gcs:send_text(1, "Not sutting down BQ40Z as vehicle is armed") + ack.result = MAV_RESULT_FAILED + mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack)) + + -- Shutdown + elseif parsed_msg.param1 == 2 then + gcs:send_text(1, "Shutting down BQ40Z!!!") + mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack)) + return shutdown_loop, 0 + + -- Pass through the command if it isn't requesting shutdown + else + local command_int = { + p1 = parsed_msg.param1, + p2 = parsed_msg.param2, + p3 = parsed_msg.param3, + p4 = parsed_msg.param4, + } + local result = gcs:run_command_int(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, command_int) + ack.result = result + mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack)) + end + end + + return update, 1000 +end + + +return update, 1000 diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 448a52c1b8184..8492fc218a7dc 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -1205,10 +1205,8 @@ int lua_GCS_command_int(lua_State *L) auto result = _gcs->lua_command_int_packet(pkt); - // map MAV_RESULT to a boolean - bool ok = result == MAV_RESULT_ACCEPTED; - - lua_pushboolean(L, ok); + // Return the resulting MAV_RESULT + lua_pushinteger(L, result); return 1; }