From c206572998aaa9595dd28043ed4e8ee69643f694 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Wed, 10 Apr 2024 17:18:28 -0700 Subject: [PATCH] wip SW-61 wip SW-61 --- .../scripts/cx_built_in_test.lua | 10 + .../generator/description/bindings.desc | 1 + scripts/cx_built_in_test.lua | 248 ++++++++++++++++++ 3 files changed, 259 insertions(+) create mode 100644 scripts/cx_built_in_test.lua diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/cx_built_in_test.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/cx_built_in_test.lua index f7675e638b..52a4a8622f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/cx_built_in_test.lua +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/cx_built_in_test.lua @@ -48,6 +48,14 @@ local servo_telem_in_error_status = {false, false, false, false, false, false, local last_motor_lost = -1 local number_of_esc = 5 + +local PARAM_TABLE_KEY = 101 +-- ******************* Parameters ******************* +assert(param:add_table(PARAM_TABLE_KEY, "CX_", 2), 'could not add param table') + +assert(param:add_param(PARAM_TABLE_KEY, 1, 'TEST', 3.14), 'could not add param1') +assert(param:add_param(PARAM_TABLE_KEY, 2, 'TEST2', 5.7), 'could not add param2') + -- ******************* Objects ******************* local auth_id = arming:get_aux_auth_id() @@ -56,6 +64,8 @@ assert(auth_id, SCRIPT_NAME .. ": could not get a prearm check auth id") local params = { -- CAN_D1_UC_ESC_BM = Parameter(), -- CAN_D2_UC_ESC_BM = Parameter(), + CX_TEST = Parameter(), + CX_TEST2 = Parameter(), EFI_TYPE = Parameter() } diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 3170f8e9a4..398ef1f642 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -254,6 +254,7 @@ 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 new file mode 100644 index 0000000000..97d6198429 --- /dev/null +++ b/scripts/cx_built_in_test.lua @@ -0,0 +1,248 @@ +--[[ + File Name: cx_built_in_test.lua + Description: This script is Carbonix Continuous Built in Test used to do multiple lua functionality of Carbonix Aircrafts, Ottano and ESC. + Functionality: + - Firmware Version base checked + - ESC Status Check and Fault Detection + - EFI Status Check and Fault Detection + Owner: [Carbonix - Software Team] +]] + + +-- ******************* Macros ******************* + +local SCRIPT_NAME = 'CX_BIT' +local CX_SANITY_SCRIPT_VERSION = 1.0 -- Script Version +local CX_PILOT_MIN_FW_VERSION = 4.4 -- Minimum Firmware Version Supported e.g Volanti 4.4.0 should be mentioned as 4.0 + + +-------- MAVLINK/AUTOPILOT 'CONSTANTS' -------- +-- local MAV_SEVERITY_EMERGENCY=0 --/* System is unusable. This is a "panic" condition. | */ +-- local MAV_SEVERITY_ALERT=1 --/* Action should be taken immediately. Indicates error in non-critical systems. | */ +local MAV_SEVERITY_CRITICAL=2 --/* Action must be taken immediately. Indicates failure in a primary system. | */ +local MAV_SEVERITY_ERROR=3 --/* Indicates an error in secondary/redundant systems. | */ +local MAV_SEVERITY_WARNING=4 --/* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ +-- local MAV_SEVERITY_NOTICE=5 --/* An unusual event has occurred though not an error condition. This should be investigated for the root cause. | */ +local MAV_SEVERITY_INFO=6 --/* Normal operational messages. Useful for logging. No action is required for these messages. | */ +-- local MAV_SEVERITY_DEBUG=7 --/* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ +-- local MAV_SEVERITY_ENUM_END=8 --/* | */ + +local MSG_NORMAL = 1 -- 1 for normal status messages +local MSG_DEBUG = 2 -- 2 for additional debug messages +local VERBOSE_MODE = 2 -- 0 to suppress all GCS messages, 1 for normal status messages, 2 for additional debug messages + +local WARNING_MSG_TIMEOUT = 15 + +local UNKNOWN = 0 +local VOLANTI = 1 +local OTTANO = 2 + +local HIRTH_EFI_TYPE = 8 +-- ******************* Variables ******************* + +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 last_motor_lost = -1 + +local number_of_esc = 5 + +local PARAM_TABLE_KEY = 101 +-- ******************* Parameters ******************* +assert(param:add_table(PARAM_TABLE_KEY, "CX_", 1), 'could not add param table') + +assert(param:add_param(PARAM_TABLE_KEY, 1, 'SERVO_ERROR', 0), 'could not add param1') + +-- ******************* Objects ******************* + +local auth_id = arming:get_aux_auth_id() +assert(auth_id, SCRIPT_NAME .. ": could not get a prearm check auth id") + +local params = { + -- CAN_D1_UC_ESC_BM = Parameter(), + -- CAN_D2_UC_ESC_BM = Parameter(), + CX_SERVO_ERROR = Parameter(), + EFI_TYPE = Parameter() +} + +-- ******************* Functions ******************* + +local function get_time_sec() + return millis():tofloat() * 0.001 +end + +-- wrapper for gcs:send_text() +local function gcs_msg(msg_type, severity, txt) + if type(msg_type) == 'string' then + -- allow just a string to be passed for simple/routine messages + txt = msg_type + msg_type = MSG_NORMAL + severity = MAV_SEVERITY_INFO + end + if msg_type <= VERBOSE_MODE then + gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt)) + end +end + +local function check_aircraft() + 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 0 +end + +local function init_param() + for paramName, param in pairs(params) do + if not param:init(paramName) then + return -1 + end + end + return 0 +end + +local function pre_arm_check_init() + -- Check Script uses a miniumum firmware version + local cx_version = FWVersion:string() + local version = cx_version:match("(%d+%.%d+)") + + -- check if firmware has CxPilot or Volanti or Ottano + if cx_version:find("CxPilot") or cx_version:find("ArduPlane") or cx_version:find("Volanti") or cx_version:find("Ottano") then + version = tonumber(version) + if CX_PILOT_MIN_FW_VERSION > version then + arming:set_aux_auth_failed(auth_id, string.format('%s Requires: %.1f. Found Version: %s', SCRIPT_NAME, CX_PILOT_MIN_FW_VERSION, version)) + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, string.format('%s Requires: %.1f. Found Version: %s', SCRIPT_NAME, CX_PILOT_MIN_FW_VERSION, version)) + return -1 + end + else + arming:set_aux_auth_failed(auth_id, string.format('Requires: CxPilot Code Base. Found Version: %s', FWVersion:string())) + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, string.format('Requires: CxPilot Code Base. Found Version: %s', FWVersion:string())) + return -1 + end + -- check param fetch + if init_param() ~= 0 then + arming:set_aux_auth_failed(auth_id, "Parameter Init Failed") + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, "Parameter Init Failed") + return -1 + end + -- check aircraft Type + if check_aircraft() ~= 0 then + arming:set_aux_auth_failed(auth_id, "Aircraft Type Check Failed") + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, "Aircraft Type Check Failed") + return -1 + end + gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "Script Version " .. CX_SANITY_SCRIPT_VERSION .. " Initialized on " .. cx_version) + arming:set_aux_auth_passed(auth_id) + return 0 +end + +local function pre_arm_check_loop() + -- check ESC + 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 + 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 + 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 + end + else + if servo_telem_in_error_status[i] == true then + gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered") + servo_telem_in_error_status[i] = false + end + servo_previous_telem_data_ms[i] = esc_last_telem_data_ms + end + end + + -- 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) + 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 + end +end + +local function during_arm_check_loop() + if arming:is_armed() then + --check ESC Telemetry is done in pre_arm_check_loop + vtol_failure_check() + end +end + +local function update() + pre_arm_check_loop() + during_arm_check_loop() +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(MSG_NORMAL, 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, 100 +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") +end + +-- local function test_print_esc_telem() +-- local val = tostring(esc_telem:get_last_telem_data_ms(0):toint()) +-- gcs:send_text(0, "ESC 0 Telemetry: " .. val) +-- val = tostring(esc_telem:get_last_telem_data_ms(1):toint()) +-- gcs:send_text(0, "ESC 1 Telemetry: " .. val) +-- val = tostring(esc_telem:get_last_telem_data_ms(2):toint()) +-- gcs:send_text(0, "ESC 2 Telemetry: " .. val) +-- val = tostring(esc_telem:get_last_telem_data_ms(3):toint()) +-- gcs:send_text(0, "ESC 3 Telemetry: " .. val) +-- return test, 500 +-- end +-- return test_print_esc_telem() + +-- ******************* Main ******************* +gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "Script Version " .. CX_SANITY_SCRIPT_VERSION .. " Running") +if pre_arm_check_init() == 0 then + return protected_wrapper() +end + +script_exit()