-
Notifications
You must be signed in to change notification settings - Fork 17.7k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
AP_Scripting: Add BQ40Z shutdown example
- Loading branch information
1 parent
f7c4d13
commit 89485d1
Showing
1 changed file
with
93 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |