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 00000000000000..1d22e2a8964e58 --- /dev/null +++ b/libraries/AP_Scripting/examples/BQ40Z_bms_shutdown.lua @@ -0,0 +1,93 @@ +-- TI BQ40Z BMS shutdown script +---@diagnostic disable: param-type-mismatch + +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 +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) + + +-- 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) + +local MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 +local MAV_RESULT_ACCEPTED = 0 + +-- Init BMS i2c device +assert(BATT_BQ40Z_BUS ~= nil, "BATT_BQ40Z_BUS not configured properly") +assert(BATT_BQ40Z_BUS:get() ~= nil, "BATT_BQ40Z_BUS not retrievable") +local bms = i2c:get_device(math.floor(BATT_BQ40Z_BUS:get()), 0x0B) +assert(bms ~= nil, "BQ40Z bms script cant create i2c device") + +-- Exit emergency shutdown (for BQ40Z60, twice for redundancy) +bms:transfer(string.pack("BBB", 0x00, 0xA7, 0x23), 0) +bms:transfer(string.pack("BBB", 0x00, 0xA7, 0x23), 0) + +-- Function that is returned to the AP scheduler when we want to shutdown +function shutdown_loop() + local ret = bms:transfer(string.pack("BBB", 0x00, 0x10, 0x00), 0) + if ret == nil then + gcs:send_text(0, "BQ40Z shutdown transfer failed") + end + + return shutdown_loop, 500 +end + +-- Main loop +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 + gcs:send_text(0, "Got MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN") + + if parsed_msg.param1 == 2 then + gcs:send_text(0, "Shutting down BQ40Z!!!") + + -- Send 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 + mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack)) + + -- Shutdown + return shutdown_loop, 0 + end + end + + return update, 1000 +end + + +return update, 1000