Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history 
SW-61
  • Loading branch information
@loki077
loki077 committed Apr 14, 2024
1 parent c206572 commit 2fa301f
Show file tree
Hide file tree
Showing 2 changed files with 131 additions and 44 deletions.
1 change: 0 additions & 1 deletion libraries/AP_Scripting/generator/description/bindings.desc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,7 +254,6 @@ singleton SRV_Channels method get_output_scaled float SRV_Channel::Aux_servo_fun
singleton SRV_Channels method set_output_norm void SRV_Channel::Aux_servo_function_t'enum SRV_Channel::k_none SRV_Channel::k_nr_aux_servo_functions-1 float -1 1
singleton SRV_Channels method set_angle void SRV_Channel::Aux_servo_function_t'enum SRV_Channel::k_none SRV_Channel::k_nr_aux_servo_functions-1 uint16_t 0 UINT16_MAX
singleton SRV_Channels method set_range void SRV_Channel::Aux_servo_function_t'enum SRV_Channel::k_none SRV_Channel::k_nr_aux_servo_functions-1 uint16_t 0 UINT16_MAX
singleton SRV_Channels manual get_safety_state SRV_Channels_get_safety_state 0

ap_object RC_Channel method norm_input float
ap_object RC_Channel method norm_input_dz float
Expand Down
174 changes: 131 additions & 43 deletions scripts/cx_built_in_test.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,47 @@ local HIRTH_EFI_TYPE = 8
local aircraft_type = UNKNOWN


local servo_previous_telem_data_ms = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
local servo_telem_in_error_status = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}
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}

-- [1] = ["Motor2", 34],
-- [2] = ["Motor3", 35],
-- [3] = ["Motor4", 36],
-- [4] = ["Motor5", 70],
-- [5] = ["Motor6", 38],
-- [6] = ["Elevator", 19],
-- [7] = ["Rudder", 21],
-- [8] = ["GPIO", -1],
-- [9] = ["Script1", 94],
-- [10] = ["Aileron", 4]
-- [11] = ["Motor12", 44],
-- [12] = ["Motor13", 45],
-- [13] = ["Motor14", 46],
-- [14] = ["Motor15", 47],
-- [15] = ["Motor16", 48]
}

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}
local last_motor_lost = -1

local number_of_esc = 5

local PARAM_TABLE_KEY = 101
local SERVO_OUT_THRESHOLD = 1150
local ESC_RPM_THRESHOLD = 10

-- ******************* Parameters *******************
assert(param:add_table(PARAM_TABLE_KEY, "CX_", 1), 'could not add param table')

Expand All @@ -67,12 +101,46 @@ local params = {
EFI_TYPE = Parameter()
}


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

local function get_time_sec()
return millis():tofloat() * 0.001
end

local function bor(x, y)
local result = 0
local bitval = 1
while x > 0 or y > 0 do
if x % 2 == 1 or y % 2 == 1 then
result = result + bitval
end
x = math.floor(x / 2)
y = math.floor(y / 2)
bitval = bitval * 2
end
return result
end

local function lshift(x, n)
return x * 2^n
end

function band(x, y)
local result = 0
local bitval = 1
while x > 0 and y > 0 do
if x % 2 == 1 and y % 2 == 1 then
result = result + bitval
end
x = math.floor(x / 2)
y = math.floor(y / 2)
bitval = bitval * 2
end
return result
end


-- wrapper for gcs:send_text()
local function gcs_msg(msg_type, severity, txt)
if type(msg_type) == 'string' then
Expand Down Expand Up @@ -137,74 +205,94 @@ local function pre_arm_check_init()
return -1
end
gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "Script Version " .. CX_SANITY_SCRIPT_VERSION .. " Initialized on " .. cx_version)
if params.CX_SERVO_ERROR:get() ~= 0 then
gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, string.format("Previous Flight SERVO ERROR: %d", params.CX_SERVO_ERROR:get()))
return 0
end
arming:set_aux_auth_passed(auth_id)
return 0
end

local function pre_arm_check_loop()
-- check ESC
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()
if esc_last_telem_data_ms == nil or esc_last_telem_data_ms == 0 or esc_last_telem_data_ms == servo_previous_telem_data_ms[i] then
if servo_telem_in_error_status[i] == false then
-- 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
if srv_telem_in_err_status[i] == false then
gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Telemetry Lost")
arming:set_aux_auth_failed(auth_id, "ESC " .. i .. " Telemetry Lost")
servo_telem_in_error_status[i] = true
params.CX_SERVO_ERROR:set(bor(math.floor(params.CX_SERVO_ERROR:get()), math.floor(lshift(1, i-1))))
srv_telem_in_err_status[i] = true
end
if esc_last_telem_data_ms ~= nil and esc_last_telem_data_ms ~= 0 then
servo_previous_telem_data_ms[i] = esc_last_telem_data_ms
srv_prv_telem_ms[i] = esc_last_telem_data_ms
end
else
if servo_telem_in_error_status[i] == true then
if arming:is_armed() then
local esc_rpm = esc_telem:get_rpm(i-1):toint()
local servo_out = SRV_Channels:get_output_pwm(srv_number[i][2])
if esc_rpm and servo_out 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")
params.CX_SERVO_ERROR:set(bor(math.floor(params.CX_SERVO_ERROR:get()), math.floor(lshift(1, i-1))))
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
end
end
end
if srv_telem_in_err_status[i] == true then
gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered")
servo_telem_in_error_status[i] = false
srv_telem_in_err_status[i] = false
end
servo_previous_telem_data_ms[i] = esc_last_telem_data_ms
srv_prv_telem_ms[i] = esc_last_telem_data_ms
end
-- RPM Drop check


end
end

local function pre_arm_check_loop()
-- 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
arming:set_aux_auth_passed(auth_id)

-- check EFI
-- if aircraft_type == OTTANO then
-- local efi_last_update_ms = tonumber(efi.get_last_update_ms(4))
-- if efi_last_update_ms == 0 or efi_last_update_ms - servo_previous_telem_data_ms[5] > 1000 then
-- arming:set_aux_auth_failed(auth_id, "EFI Telemetry Lost")
-- gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "EFI Telemetry Lost")
-- return -1
-- end
-- servo_previous_telem_data_ms[5] = efi_last_update_ms
-- end

-- check servo

for i, status in ipairs(servo_telem_in_error_status) do
if status == false then
arming:set_aux_auth_passed(auth_id)
for i, status in ipairs(srv_telem_in_err_status) do
gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, string.format("Servo %d: ", i))
gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, string.format("Servo %d: %d", i, params.CX_SERVO_ERROR:get()))
if status == true or band(bit.lshift(params.CX_SERVO_ERROR:get(), i-1), 1) == 1 then
arming:set_aux_auth_failed(auth_id, "Actuator ".. i .. " Telemetry Error")
return
end
end

return 0
end

local function vtol_failure_check()
local lost_index = MotorsMatrix:get_thrust_boost() and MotorsMatrix:get_lost_motor() or -1
if lost_index ~= last_motor_lost then
local message = lost_index == -1 and "Motors Thrust: recovered" or "Motor ".. lost_index + 1 .." Thrust: lost"
local severity = lost_index == -1 and MAV_SEVERITY_INFO or MAV_SEVERITY_CRITICAL
gcs_msg(MSG_NORMAL, severity, message)
last_motor_lost = lost_index
if pre_arm_status == false then
arming:set_aux_auth_passed(auth_id)
end

end

local function during_arm_check_loop()
local function vtol_failure_check()
if arming:is_armed() then
--check ESC Telemetry is done in pre_arm_check_loop
vtol_failure_check()
local lost_index = MotorsMatrix:get_thrust_boost() and MotorsMatrix:get_lost_motor() or -1
if lost_index ~= last_motor_lost then
local message = lost_index == -1 and "Motors Thrust: recovered" or "Motor ".. lost_index + 1 .." Thrust: lost"
local severity = lost_index == -1 and MAV_SEVERITY_INFO or MAV_SEVERITY_CRITICAL
gcs_msg(MSG_NORMAL, severity, message)
last_motor_lost = lost_index
end
end
end


local function update()
esc_check_loop()
vtol_failure_check()
pre_arm_check_loop()
during_arm_check_loop()
end

-- wrapper around update(). This calls update() and if update faults
Expand Down

0 comments on commit 2fa301f

Please sign in to comment.