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

cx_built_in_test: debounce audible warning for nil #146

Merged
merged 2 commits into from
Jun 4, 2024
Merged

cx_built_in_test: debounce audible warning for nil #146

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2a11455
cx_built_in_test: debounce audible warning for nil
robertlong13 Jun 4, 2024
def5980
cx_built_in_test: fixup telem sanity check order
loki077 Jun 4, 2024
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
73 changes: 49 additions & 24 deletions libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/cx_built_in_test.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,50 +118,75 @@ local function esc_is_stopped(i)
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
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 not esc_warmup_end_time[i] 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] 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
-- 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(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
Expand Down
Loading