diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 398ef1f642..3170f8e9a4 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -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 diff --git a/scripts/cx_built_in_test.lua b/scripts/cx_built_in_test.lua index 97d6198429..bed6c8517c 100644 --- a/scripts/cx_built_in_test.lua +++ b/scripts/cx_built_in_test.lua @@ -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') @@ -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 @@ -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