Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

LUA : restructure to manage prearm-checks #261

Open
wants to merge 1 commit into
base: CxPilot-7
Choose a base branch
from
+366 −243
Open

LUA : restructure to manage prearm-checks #261

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
342 changes: 99 additions & 243 deletions libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,243 +1,99 @@
--[[
File Name: cx_built_in_test.lua
Description: This script performs a continuous built-in test for various functionalities on Carbonix Aircrafts, focusing on ESC status check and fault detection.
Owner: [Carbonix - Software Team]
]]

-- ******************* Macros *******************

local SCRIPT_NAME = 'CX_BIT'
local SCRIPT_VERSION = 1.0 -- Script Version


-------- MAVLINK/AUTOPILOT 'CONSTANTS' --------
-- MAVLink Severity Levels
local MAV_SEVERITY_CRITICAL = 2
local MAV_SEVERITY_ERROR = 3
local MAV_SEVERITY_WARNING = 4
local MAV_SEVERITY_INFO = 6

-- Engine Types
local HIRTH_EFI_TYPE = 8

local ESC_WARMUP_TIME = 3000
local SERVO_OUT_THRESHOLD = 1010
local ESC_RPM_THRESHOLD = 10
-- ******************* Variables *******************

local number_of_esc = 5 --default value for Volanti

-- Add a new table to store the warm-up end times for each ESC
local esc_warmup_end_time = {}

local srv_number = {
[1] = {"Motor1", 33},
[2] = {"Motor2", 34},
[3] = {"Motor3", 35},
[4] = {"Motor4", 36},
[5] = {"Motor5", 70},
[6] = {"Motor6", 38},
[7] = {"Elevator", 19},
[8] = {"Rudder", 21},
[9] = {"GPIO", -1},
[10] = {"Script1", 94},
[11] = {"Aileron", 4}
}

local srv_prv_telem_ms = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
local srv_telem_in_err_status = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}
local srv_rpm_in_err_status = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}

-- ******************* Objects *******************

local auth_id = arming:get_aux_auth_id()
assert(auth_id, SCRIPT_NAME .. ": could not get a prearm check auth id")

local params = {
EFI_TYPE = Parameter()
}


-- ******************* Functions *******************

-- wrapper for gcs:send_text()
local function gcs_msg(severity, txt)
if type(severity) == 'string' then
-- allow just a string to be passed for simple/routine messages
txt = severity
severity = MAV_SEVERITY_INFO
end
gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt))
end

local function check_aircraft_type()
if params.EFI_TYPE:get() == HIRTH_EFI_TYPE then
number_of_esc = 4
else
number_of_esc = 5
end
return true
end

local function init_param()
for param_name, param in pairs(params) do
if not param:init(param_name) then
return false
end
end
return true
end

local function arming_check_init()
-- check param fetch
if not init_param() then
arming:set_aux_auth_failed(auth_id, "Parameter Init Failed")
gcs_msg(MAV_SEVERITY_WARNING, "Parameter Init Failed")
return false
end
-- check aircraft Type
if not check_aircraft_type() then
arming:set_aux_auth_failed(auth_id, "Aircraft Type Check Failed")
gcs_msg(MAV_SEVERITY_WARNING, "Aircraft Type Check Failed")
return false
end
gcs_msg(MAV_SEVERITY_INFO, "Script Version " .. SCRIPT_VERSION .. " Initialized")
arming:set_aux_auth_passed(auth_id)
return true
end

-- Call this function whenever a motor starts running
local function esc_is_started(i)
-- Set the warm-up end time for this ESC to 3 seconds from now
esc_warmup_end_time[i] = millis() + ESC_WARMUP_TIME
end

-- Call this function whenever a motor stops running
local function esc_is_stopped(i)
-- Clear the warm-up end time for this ESC
esc_warmup_end_time[i] = nil
end

-- Counters to debounce nil checks on esc rpm and servo output, this is a
-- workaround to avoid giving the pilot a critical warning for an unexplained
-- one-loop dropout we saw recently
local NIL_WARN_THRESHOLD = 3
local esc_rpm_nil_counter = {0, 0, 0, 0, 0}
local servo_out_nil_counter = {0, 0, 0, 0, 0}

local function esc_check_loop()
for i = 1, number_of_esc do
local esc_last_telem_data_ms = esc_telem:get_last_telem_data_ms(i-1):toint()
local esc_rpm = esc_telem:get_rpm(i-1)
local servo_out = SRV_Channels:get_output_pwm(srv_number[i][2])
-- Telem data timestamp check
if not esc_last_telem_data_ms or esc_last_telem_data_ms == 0 or esc_last_telem_data_ms == srv_prv_telem_ms[i] then
if srv_telem_in_err_status[i] == false then
gcs_msg(MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Telemetry Lost")
srv_telem_in_err_status[i] = true
end
-- Nil check for RPM reading
elseif not esc_rpm then
esc_rpm_nil_counter[i] = esc_rpm_nil_counter[i] + 1
if esc_rpm_nil_counter[i] < NIL_WARN_THRESHOLD then
gcs_msg(MAV_SEVERITY_INFO, "ESC " .. i .. " RPM nil")
elseif srv_rpm_in_err_status[i] == false then
gcs_msg(MAV_SEVERITY_CRITICAL, "ESC " .. i .. " RPM nil")
srv_telem_in_err_status[i] = true
end
-- Nil check for servo output
elseif not servo_out then
servo_out_nil_counter[i] = servo_out_nil_counter[i] + 1
if servo_out_nil_counter[i] < NIL_WARN_THRESHOLD then
gcs_msg(MAV_SEVERITY_INFO, "ESC " .. i .. " Servo Out nil")
elseif srv_rpm_in_err_status[i] == false then
gcs_msg(MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Servo Out nil")
srv_telem_in_err_status[i] = true
end
-- Telemetry data is fresh and valid
else
if srv_telem_in_err_status[i] == true then
gcs_msg(MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered")
srv_telem_in_err_status[i] = false
servo_out_nil_counter[i] = 0
esc_rpm_nil_counter[i] = 0
end
-- If armed, check that the motor is actually turning when it is commanded to
if arming:is_armed() then
-- If the PWM is below the threshold, it is okay for the motor to be stopped
if servo_out < SERVO_OUT_THRESHOLD then
esc_is_stopped(i)
-- If the PWM has just gone above the threshold, start the warm-up timer
elseif servo_out > SERVO_OUT_THRESHOLD and not esc_warmup_end_time[i] then
esc_is_started(i)
-- If the motor is running, and the warmup timer has expired, check that the motor is spinning
elseif esc_warmup_end_time[i] and millis() > esc_warmup_end_time[i] then
if servo_out > SERVO_OUT_THRESHOLD and esc_rpm < ESC_RPM_THRESHOLD then
if srv_rpm_in_err_status[i] == false then
gcs_msg(MAV_SEVERITY_CRITICAL, "ESC " .. i .. " RPM Drop")
srv_rpm_in_err_status[i] = true
end
else
if srv_rpm_in_err_status[i] == true then
gcs_msg(MAV_SEVERITY_INFO, "ESC " .. i .. " RPM Recovered")
srv_rpm_in_err_status[i] = false
end
end
end
end
end
-- Update srv_prv_telem_ms[i] if it had valid data this loop
if esc_last_telem_data_ms and esc_last_telem_data_ms ~= 0 then
srv_prv_telem_ms[i] = esc_last_telem_data_ms
end
end
end

local function arming_checks()
-- check for status in srv_telem_in_err_status and also CX_SERVO_ERROR bit status
local pre_arm_status = false-- check for status in srv_telem_in_err_status and also CX_SERVO_ERROR bit status
arming:set_aux_auth_passed(auth_id)

for i, status in ipairs(srv_telem_in_err_status) do
if status == true then
arming:set_aux_auth_failed(auth_id, "Actuator ".. i .. " Telemetry Error")
return false
end
end
if pre_arm_status == false then
arming:set_aux_auth_passed(auth_id)
end

end

local function update()
esc_check_loop()
arming_checks()
end

-- wrapper around update(). This calls update() and if update faults
-- then an error is displayed, but the script is not stopped
local function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs_msg(MAV_SEVERITY_ERROR, "Internal Error: " .. err)
-- when we fault we run the update function again after 1s, slowing it
-- down a bit so we don't flood the console with errors
return protected_wrapper, 1000
end
return protected_wrapper, 200
end

local function script_exit()
-- pre arm failure SCRIPT_NAME not Running
arming:set_aux_auth_failed(auth_id, SCRIPT_NAME .. " Not Running")
gcs_msg(MAV_SEVERITY_CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize")
end


-- ******************* Main *******************
if arming_check_init() then
return protected_wrapper, 10000
end

script_exit()
local cx_msg = require("scripts.cx_utils.cx_msg")
local cx_esc = require("scripts.cx_modules.esc")

-- Create instances of modules
cx_msg = cx_msg.new()

-- Add modules that require Built-in-test (implemented in cx_modules)
-- Each module should have the following functions:
-- 1. new() - constructor
-- 2. init() - initialize the module
-- 3. update() - update the module
-- 4. check_for_errors() - returns pre-arm checks/errors in the module
-- - built in test errors are managed by each module
local modules = {
esc = cx_esc.new()
}

-- ******************* Functions *******************
-- initialize function
local function init()
-- initialize all modules
for _, module in pairs(modules) do
module:init()
end

cx_msg:send(cx_msg.MAV_SEVERITY_INFO, "LUA script initialized")
return true
end

-- Pre-arm status check before arming
local function check_prearm_status()
-- Track errors in modules
local modules_with_errors = {}
local total_errors = 0

for _, module in pairs(modules) do
local error_count = module:check_for_errors()
if error_count > 0 then
table.insert(modules_with_errors, { name = module.name, errors = error_count })
total_errors = total_errors + error_count
end
end

-- Handle error cases
if #modules_with_errors == 0 then
-- No modules have errors, clear error flag
cx_msg:clear_prearm_error()
elseif #modules_with_errors == 1 then
-- Only one module has errors
local module = modules_with_errors[1]
msg = module.name .. " (" .. module.errors .. " checks fail)"
cx_msg:set_prearm_error(msg)
else
-- More than one module has errors
msg = #modules_with_errors .. " modules (" .. total_errors .. " checks) failing. Check messages"
cx_msg:set_prearm_error(msg)
end
end

-- update function
local function update()
-- update all modules
for _, module in pairs(modules) do
module:update()
end

-- check for any prearm errors
if not arming:is_armed() then
check_prearm_status()
end
end

-- wrapper around update(). This calls update() and if update faults
-- then an error is displayed, but the script is not stopped
local function protected_wrapper()
local success, err = pcall(update)
if not success then
cx_msg:send(cx_msg.MAV_SEVERITY_ERROR, "Internal Error: " .. err)
-- when we fault we run the update function again after 1s, slowing it
-- down a bit so we don't flood the console with errors
return protected_wrapper, 1000
end
return protected_wrapper, 200
end

-- exit function
local function script_exit()
-- pre arm failure SCRIPT_NAME not Running
arming:set_aux_auth_failed(auth_id, SCRIPT_NAME .. " Not Running")
cx_msg:send(cx_msg.MAV_SEVERITY_CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize")
end


-- ******************* Main *******************
if init() then
return protected_wrapper, 10000
end

script_exit()
Loading