From ecb8b810c7745635792a38d15961490f351cd783 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Sun, 15 Dec 2024 23:28:24 -0500 Subject: [PATCH] AP_Scripting: Add example for health monitoring --- .../AP_Scripting/examples/health_check.lua | 160 ++++++++++++++++++ 1 file changed, 160 insertions(+) create mode 100644 libraries/AP_Scripting/examples/health_check.lua diff --git a/libraries/AP_Scripting/examples/health_check.lua b/libraries/AP_Scripting/examples/health_check.lua new file mode 100644 index 0000000000..814287999d --- /dev/null +++ b/libraries/AP_Scripting/examples/health_check.lua @@ -0,0 +1,160 @@ +-- Sample health monitoring script that will monitor some key subsytems when the vehicle is armed. +-- If any of the subsystems are unhealthy, the script will prevent the vehicle from arming the next time by setting a bit in the HEALTH_STATUS bitmask. + +-- Define parameter table keys and prefix +local PARAM_TABLE_KEY = 157 +local PARAM_TABLE_PREFIX = "HEALTH_" + +-- Global arming authorization ID +local auth_id = arming:get_aux_auth_id() + +-- Bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('Could not find %s parameter', name)) + return p +end + +-- Add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('Could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +local STATS_FLTTIME = bind_param("STAT_FLTTIME") + +-- Setup parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 11), 'Could not add param table') + +-- Define the enable parameter +--[[ + // @Param: HEALTH_ENABLE + // @DisplayName: Enable Health Monitoring Script + // @Description: Enable or disable the health monitoring script. Set to 1 to enable, 0 to disable. + // @Values: 0:Disable, 1:Enable + // @User: Standard +]] +local HEALTH_ENABLE = bind_add_param("ENABLE", 1, 0) + +-- Define the bitmask parameter +--[[ + // @Param: HEALTH_STATUS + // @DisplayName: System Health Bitmask + // @Description: Bitmask representing the health of various subsystems. Each bit indicates the health of a subsystem. + // @Bitmask: 0:EKF, 1:Total flight time, 2:Other subsystem + // @User: Advanced +]] +local HEALTH_STATUS = bind_add_param("STATUS", 2, 0) -- Default is all systems healthy + +------------------- Various subsystem health-check functions ------------------- +-- Subsystem health-check functions +local function check_ekf_health() + -- Get variances from AHRS + local vel_variance, pos_variance, height_variance, mag_variance, airspeed_variance = ahrs:get_variances() + -- Check if any variance exceeds 1.5 + if vel_variance > 1.5 or pos_variance > 1.5 or height_variance > 1.5 or mag_variance:length() > 1.5 or airspeed_variance > 1.5 then + return false + end + return true +end + +local function check_total_flight_time() + -- Fetch total flight time from STAT_FLTTIME + local total_flight_time = STATS_FLTTIME:get() -- Flight time in seconds + + -- Define a threshold for the maximum allowable flight time (e.g., 1,000,000 seconds, or ~277.7 hours) + local max_flight_time = 1000000 + + if total_flight_time > max_flight_time then + return false + end + return true +end + +local function check_other_health() + -- Placeholder for additional subsystem checks + return true +end + +------------------- Subsystem definitions ------------------- + +-- Subsystem definitions (bit, description, check function). Add more subsystems as needed. Add corresponding check functions above. +local subsystems = { + {bit = 0, name = "EKF health", check = check_ekf_health}, + {bit = 1, name = "Total flight time", check = check_total_flight_time}, + {bit = 2, name = "Other subsystem", check = check_other_health} +} + +------------------- Main Logic ------------------- + +-- Utility function to set or clear a bit in the bitmask +local function set_bit(mask, bit, healthy) + if healthy then + return mask & ~(1 << bit) -- Clear the bit if healthy (0) + else + return mask | (1 << bit) -- Set the bit if unhealthy (1) + end +end + +-- Function to check health of subsystems and update the bitmask +local function check_health() + local bitmask = HEALTH_STATUS:get() -- Fetch the current bitmask + + for _, subsystem in ipairs(subsystems) do + local is_healthy = subsystem.check() + bitmask = set_bit(bitmask, subsystem.bit, is_healthy) + end + + -- Update the HEALTH_STATUS parameter + HEALTH_STATUS:set(bitmask) + + -- Notify if any subsystem is unhealthy + for _, subsystem in ipairs(subsystems) do + if (bitmask & (1 << subsystem.bit)) ~= 0 then -- Bit is 1, unhealthy + gcs:send_text(5, "Warning: " .. subsystem.name .. " is unhealthy!") + end + end +end + +-- Function to handle arming checks +local function handle_arming() + local bitmask = HEALTH_STATUS:get() + local arming_failed = false + + for _, subsystem in ipairs(subsystems) do + if (bitmask & (1 << subsystem.bit)) ~= 0 then -- Bit is 1, unhealthy + gcs:send_text(5, "Arming denied: " .. subsystem.name .. " check failed.") + arming_failed = true + end + end + + if arming_failed then + arming:set_aux_auth_failed(auth_id, "Health checks failed") + else + arming:set_aux_auth_passed(auth_id) + end +end + +-- Main update function +function update() + -- Check if health monitoring is enabled + if HEALTH_ENABLE:get() == 0 then + arming:set_aux_auth_passed(auth_id) -- Allow arming if script is disabled + return update, 1000 -- Check again in 1 second + end + + if arming:is_armed() then + check_health() -- Periodic health check while armed + else + -- When disarmed, save the updated bitmask permanently + HEALTH_STATUS:set_and_save(HEALTH_STATUS:get()) + -- Handle arming checks + handle_arming() + end + + return update, 1000 -- Run update every second +end + +-- Initialize +gcs:send_text(6, "Health monitoring script initialized.") +return update()