Skip to content

Commit

Permalink
cx_built_in_test : Multiple Fixes backporting from CxPilot
Browse files Browse the repository at this point in the history 
There are same major changes done in CxPilot plus the below changes to be backported.

2a11455
def5980

SW-219
  • Loading branch information
@loki077
loki077 committed Jun 4, 2024
1 parent 2e77acc commit 2680c0a
Showing 1 changed file with 70 additions and 62 deletions.
132 changes: 70 additions & 62 deletions libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/cx_built_in_test.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,15 +17,6 @@ local MAV_SEVERITY_ERROR = 3
local MAV_SEVERITY_WARNING = 4
local MAV_SEVERITY_INFO = 6

-- Messaging Levels
local MSG_NORMAL = 1
local MSG_DEBUG = 2
local VERBOSE_MODE = 2 -- 0: No messages, 1: Normal, 2: Debug

-- Aircraft Types
local VOLANTI = 1
local OTTANO = 2

-- Engine Types
local HIRTH_EFI_TYPE = 8

Expand All @@ -34,7 +25,6 @@ local SERVO_OUT_THRESHOLD = 1010
local ESC_RPM_THRESHOLD = 10
-- ******************* Variables *******************

local aircraft_type = VOLANTI
local number_of_esc = 5 --default value for Volanti

-- Add a new table to store the warm-up end times for each ESC
Expand Down Expand Up @@ -71,24 +61,19 @@ local params = {
-- ******************* Functions *******************

-- wrapper for gcs:send_text()
local function gcs_msg(msg_type, severity, txt)
if type(msg_type) == 'string' then
local function gcs_msg(severity, txt)
if type(severity) == 'string' then
-- allow just a string to be passed for simple/routine messages
txt = msg_type
msg_type = MSG_NORMAL
txt = severity
severity = MAV_SEVERITY_INFO
end
if msg_type <= VERBOSE_MODE then
gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt))
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
aircraft_type = OTTANO
number_of_esc = 4
else
aircraft_type = VOLANTI
number_of_esc = 5
end
return true
Expand All @@ -105,91 +90,115 @@ end

local function arming_check_init()
-- check param fetch
if not init_param() then
if not init_param() then
arming:set_aux_auth_failed(auth_id, "Parameter Init Failed")
gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, "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(MSG_NORMAL, MAV_SEVERITY_WARNING, "Aircraft Type Check Failed")
gcs_msg(MAV_SEVERITY_WARNING, "Aircraft Type Check Failed")
return false
end
gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "Script Version " .. SCRIPT_VERSION .. " Initialized")
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
function esc_is_started(i)
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
function esc_is_stopped(i)
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()
-- Telem data timesatmp check
if esc_last_telem_data_ms == nil or esc_last_telem_data_ms == 0 or esc_last_telem_data_ms == srv_prv_telem_ms[i] then
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(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Telemetry Lost")
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
if esc_last_telem_data_ms ~= nil and esc_last_telem_data_ms ~= 0 then
srv_prv_telem_ms[i] = esc_last_telem_data_ms
-- 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(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered")
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
local esc_rpm = esc_telem:get_rpm(i-1)
local servo_out = SRV_Channels:get_output_pwm(srv_number[i][2])
if esc_rpm and servo_out then
-- If the PWM is below the threshold, consider it a stopped motor situation
if servo_out < SERVO_OUT_THRESHOLD then
esc_is_stopped(i)
-- If the PWM is above the threshold consider it start the motor
elseif servo_out > SERVO_OUT_THRESHOLD and esc_warmup_end_time[i] == nil then
esc_is_started(i)
-- If the motor is running and the PWM is above the threshold, check the RPM
elseif esc_warmup_end_time[i] ~= nil 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(MSG_NORMAL, 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(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " RPM Recovered")
srv_rpm_in_err_status[i] = false
end
-- 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
else
gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Null Rcout or RPM")
srv_telem_in_err_status[i] = true
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
pre_arm_status = false-- 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

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
Expand All @@ -198,9 +207,8 @@ local function arming_checks()
if pre_arm_status == false then
arming:set_aux_auth_passed(auth_id)
end

end

end

local function update()
esc_check_loop()
Expand All @@ -212,7 +220,7 @@ end
local function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs_msg(MSG_NORMAL, MAV_SEVERITY_ERROR, "Internal Error: " .. err)
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
Expand All @@ -223,7 +231,7 @@ 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(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize")
gcs_msg(MAV_SEVERITY_CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize")
end


Expand Down

0 comments on commit 2680c0a

Please sign in to comment.