diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index b3a710ab98..a87dc2e2f6 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -440,19 +440,49 @@ def __init__(self): "waf_target": "bin/arduplane", "model": "quadplane:@ROMFS/models/Ottano.json", "default_params_filename": "../../build/ottano-headless.parm", - "scripts": ["../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua"], + "scripts": [ + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua", + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/*.lua", + "modules" + ), + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua", + "modules/aircraft.lua" + ), + ], }, "volanti-headless": { "waf_target": "bin/arduplane", "model": "quadplane:@ROMFS/models/Volanti.json", "default_params_filename": "../../build/volanti-headless.parm", - "scripts": ["../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua"], + "scripts": [ + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua", + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/*.lua", + "modules" + ), + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua", + "modules/aircraft.lua" + ), + ], }, "volanti-realflight": { "waf_target": "bin/arduplane", "model": "flightaxis", "default_params_filename": "../../build/volanti-realflight.parm", - "scripts": ["../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua"], + "scripts": [ + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/*.lua", + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/*.lua", + "modules" + ), + ( + "../../libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua", + "modules/aircraft.lua" + ), + ], "external": True, } }, diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml index b1d75ed9ae..8f441aceb1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml @@ -139,5 +139,17 @@ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua cx_vtol_info.lua + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua + modules/bit_esc.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua + modules/msg.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua + modules/aircraft.lua + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml index 55fdb65a92..c565c47b38 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml @@ -139,5 +139,17 @@ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua cx_vtol_info.lua + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua + modules/bit_esc.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua + modules/msg.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua + modules/aircraft.lua + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml index aa31ad8db9..8b3697f4a0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml @@ -109,5 +109,17 @@ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua cx_vtol_info.lua + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua + modules/bit_esc.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua + modules/msg.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua + modules/aircraft.lua + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml index defebe67cd..a762c47472 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml @@ -120,5 +120,17 @@ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua cx_vtol_info.lua + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua + modules/bit_esc.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua + modules/msg.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua + modules/aircraft.lua + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml index ca8e84e05b..c334ec568a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml @@ -120,5 +120,17 @@ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua cx_vtol_info.lua + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua + modules/bit_esc.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua + modules/msg.lua + + + libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua + modules/aircraft.lua + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm index 35f89b22d0..b87fc6d969 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm @@ -119,8 +119,8 @@ RTL_RADIUS,200 SCHED_LOOP_RATE,200 # Loop Rate for Control in flight Controller set to 200Hz from 400Hz Results SW-171. SCR_ENABLE,1 SCR_HEAP_SIZE,200000 -SCR_LD_CHECKSUM,8357358 -SCR_RUN_CHECKSUM,8357358 +SCR_LD_CHECKSUM,1341533 +SCR_RUN_CHECKSUM,1341533 SCR_VM_I_COUNT,100000 SERVO1_FUNCTION,33 # Motor 1 SERVO10_FUNCTION,4 # Aileron R diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua new file mode 100644 index 0000000000..fd1588fb20 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/ottano.lua @@ -0,0 +1 @@ +return "Ottano" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua new file mode 100644 index 0000000000..ce75cf043f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/aircraft/volanti.lua @@ -0,0 +1 @@ +return "Volanti" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua index 87c14dce44..5799ca014f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua @@ -1,242 +1,143 @@ ---[[ - File Name: cx_built_in_test.lua - Description: This script performs a continuous built-in test for various functionalities on Carbonix Aircrafts, focusing on ESC status check and fault detection. - Owner: [Carbonix - Software Team] -]] - --- ******************* Macros ******************* - -local SCRIPT_NAME = 'CX_BIT' -local SCRIPT_VERSION = 1.0 -- Script Version - - --------- MAVLINK/AUTOPILOT 'CONSTANTS' -------- --- MAVLink Severity Levels -local MAV_SEVERITY_CRITICAL = 2 -local MAV_SEVERITY_ERROR = 3 -local MAV_SEVERITY_WARNING = 4 -local MAV_SEVERITY_INFO = 6 - -local ESC_WARMUP_TIME = 3000 -local SERVO_OUT_THRESHOLD = 1010 -local ESC_RPM_THRESHOLD = 10 --- ******************* Variables ******************* - -local number_of_esc = 5 --default value for Volanti - --- Add a new table to store the warm-up end times for each ESC -local esc_warmup_end_time = {} - -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} -} - -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} - --- ******************* Objects ******************* - -local auth_id = arming:get_aux_auth_id() -assert(auth_id, SCRIPT_NAME .. ": could not get a prearm check auth id") - -local params = { - EFI_TYPE = Parameter() -} - - --- ******************* Functions ******************* - --- wrapper for gcs:send_text() -local function gcs_msg(severity, txt) - if type(severity) == 'string' then - -- allow just a string to be passed for simple/routine messages - txt = severity - severity = MAV_SEVERITY_INFO - end - gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt)) -end - -local function check_aircraft_type() - if params.EFI_TYPE:get() ~= 0 then - number_of_esc = 4 - else - number_of_esc = 5 - end - return true -end - -local function init_param() - for param_name, param in pairs(params) do - if not param:init(param_name) then - return false - end - end - return true -end - -local function arming_check_init() - -- check param fetch - if not init_param() then - arming:set_aux_auth_failed(auth_id, "Parameter Init Failed") - gcs_msg(MAV_SEVERITY_WARNING, "Parameter Init Failed") - return false - end - -- check aircraft Type - if not check_aircraft_type() then - arming:set_aux_auth_failed(auth_id, "Aircraft Type Check Failed") - gcs_msg(MAV_SEVERITY_WARNING, "Aircraft Type Check Failed") - return false - end - gcs_msg(MAV_SEVERITY_INFO, "Script Version " .. SCRIPT_VERSION .. " Initialized") - arming:set_aux_auth_passed(auth_id) - return true -end - --- Call this function whenever a motor starts running -local function esc_is_started(i) - -- Set the warm-up end time for this ESC to 3 seconds from now - esc_warmup_end_time[i] = millis() + ESC_WARMUP_TIME -end - --- Call this function whenever a motor stops running -local function esc_is_stopped(i) - -- Clear the warm-up end time for this ESC - 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 - -- 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 - end - else - esc_is_stopped(i) - 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 - end -end - -local function arming_checks() - -- check for status in srv_telem_in_err_status and also CX_SERVO_ERROR bit status - local 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) - - for i, status in ipairs(srv_telem_in_err_status) do - if status == true then - arming:set_aux_auth_failed(auth_id, "Actuator ".. i .. " Telemetry Error") - return false - end - end - if pre_arm_status == false then - arming:set_aux_auth_passed(auth_id) - end - -end - -local function update() - esc_check_loop() - arming_checks() -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(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, 200 -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(MAV_SEVERITY_CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize") -end - - --- ******************* Main ******************* -if arming_check_init() then - return protected_wrapper, 10000 -end - -script_exit() +local cx_msg = require("msg") +local cx_esc = require("bit_esc") + +-- Add subsystems that require Built-in-test (implemented in subsystems) +-- Each subsystem should have the following functions: +-- 1. init() - initialize the subsystem +-- 2. update() - update the subsystem +-- 3. check_for_errors() - returns pre-arm checks/errors in the subsystem +-- - built in test errors are managed by each subsystem +local subsystems = { + cx_esc, +} + +-- auth id for prearm check +local prearm_msg = nil +local auth_id = arming:get_aux_auth_id() +assert(auth_id, SCRIPT_NAME .. ": could not get prearm check auth id") + +-- ******************* Functions ******************* +-- get time in seconds since boot +local function get_time() + return millis():tofloat() * 0.001 +end + +local function set_prearm_error(txt) + if (not prearm_msg) or (prearm_msg ~= txt) then + prearm_msg = txt + arming:set_aux_auth_failed(auth_id, txt) + end +end + +local function clear_prearm_error() + prearm_msg = nil + arming:set_aux_auth_passed(auth_id) +end + +-- initialize function +local function init() + -- initialize all subsystems that are part of constructor + for _, subsystem in pairs(subsystems) do + subsystem:init() + end + + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "LUA script initialized") + return true +end + +-- Pre-arm status check before arming +local last_prearm_msg_s = 0 -- timestamp of last message sent +local prearm_messages = {} -- Set of all unique messages seen since we last sent +local function check_prearm_status() + -- Track errors in subsystems + local subsystems_with_errors = {} + local msg = "" + for _, subsystem in pairs(subsystems) do + local errors = subsystem:check_for_errors() + if #errors > 0 then + table.insert(subsystems_with_errors, subsystem.name) + for _, error in pairs(errors) do + prearm_messages[error] = true + end + if #errors == 1 then + msg = errors[1] + else + msg = errors .. " " .. subsystem.name .. " errors. Check messages." + end + end + end + + -- Handle prearm + if #subsystems_with_errors == 0 then + clear_prearm_error() + elseif #subsystems_with_errors == 1 then + set_prearm_error(msg) + else + msg = "" + for _, subsystem in pairs(subsystems_with_errors) do + msg = msg .. subsystem .. ", " + end + msg = msg:sub(1, -3) .. " failing. Check messages." + set_prearm_error(msg) + end + + -- Every 2 seconds, send a message with all the unique errors seen. This + -- helps the operator see the specific errors if there are more than one + -- (since the prearm library only allows one error message for all scripts + -- to share) + if get_time() - last_prearm_msg_s > 2 or get_time() < last_prearm_msg_s then + -- Count the number of unique errors (# operator doesn't work on sets) + local num_prearm_errors = 0 + for _ in pairs(prearm_messages) do + num_prearm_errors = num_prearm_errors + 1 + end + if num_prearm_errors > 1 then + for err, _ in pairs(prearm_messages) do + gcs:send_text(cx_msg.MAV_SEVERITY.CRITICAL, "Prearm: " .. err) + end + end + last_prearm_msg_s = get_time() + prearm_messages = {} + end +end + +-- update function +local function update() + -- update all subsystems + for _, subsystem in pairs(subsystems) do + subsystem:update() + end + + -- check for any prearm errors + if not arming:is_armed() then + check_prearm_status() + end +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 + cx_msg:send(cx_msg.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, 200 +end + +-- exit function +local function script_exit() + -- pre arm failure SCRIPT_NAME not Running + arming:set_aux_auth_failed(auth_id, SCRIPT_NAME .. " Not Running") + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "LUA SCRIPT EXIT ... Need Reboot to Reinitialize") +end + + +-- ******************* Main ******************* +if init() then + return protected_wrapper, 10000 +end + +script_exit() diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua new file mode 100644 index 0000000000..eadc59ae5a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua @@ -0,0 +1,161 @@ +local cx_msg = require("msg") +local aircraft_type = require("aircraft") + +local ESC = { + name = "ESC", + + number_of_esc = 5, + + -- CONSTANTS + ESC_WARMUP_TIME = 3000, + ESC_RPM_THRESHOLD = 10, + SERVO_OUT_THRESHOLD = 1010, + + -- Add a new table to store the warm-up end times for each ESC + esc_warmup_end_time = {}, + + srv_prv_telem_ms = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + srv_telem_in_err_status = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}, + srv_rpm_in_err_status = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}, + + -- 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 + NIL_WARN_THRESHOLD = 3, + esc_rpm_nil_counter = {0, 0, 0, 0, 0}, + servo_out_nil_counter = {0, 0, 0, 0, 0}, + + 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} + }, +} + +-- Get the number of ESCs based on the aircraft type +function ESC:get_num_esc() + if aircraft_type == "Volanti" then + self.number_of_esc = 5 + elseif aircraft_type == "Ottano" then + self.number_of_esc = 4 + else + assert(false, "ESC init failed: Aircraft type not set") + end +end + +-- Initialize the ESC module +function ESC:init() + self:get_num_esc() + if self.number_of_esc == 0 then + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "ESC init failed: Aircraft type not set") + return + end + for i = 1, self.number_of_esc do + self.esc_warmup_end_time[i] = nil + self.srv_prv_telem_ms[i] = 0 + end + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "ESC init (" .. aircraft_type .. ": " .. self.number_of_esc .. " ESCs)") +end + + +-- Call this function whenever a motor starts running +function ESC:esc_is_started(i) + -- Set the warm-up end time for this ESC to 3 seconds from now + self.esc_warmup_end_time[i] = millis() + self.ESC_WARMUP_TIME +end + +-- Call this function whenever a motor stops running +function ESC:esc_is_stopped(i) + -- Clear the warm-up end time for this ESC + self.esc_warmup_end_time[i] = nil +end + +function ESC:update() + for i = 1, self.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(self.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 == self.srv_prv_telem_ms[i] then + if self.srv_telem_in_err_status[i] == false then + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "ESC " .. i .. " Telemetry Lost") + self.srv_telem_in_err_status[i] = true + end + -- Nil check for RPM reading + elseif not esc_rpm then + self.esc_rpm_nil_counter[i] = self.esc_rpm_nil_counter[i] + 1 + if self.esc_rpm_nil_counter[i] < self.NIL_WARN_THRESHOLD then + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "ESC " .. i .. " RPM nil") + elseif self.srv_rpm_in_err_status[i] == false then + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "ESC " .. i .. " RPM nil") + self.srv_telem_in_err_status[i] = true + end + -- Nil check for servo output + elseif not servo_out then + self.servo_out_nil_counter[i] = self.servo_out_nil_counter[i] + 1 + if self.servo_out_nil_counter[i] < self.NIL_WARN_THRESHOLD then + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "ESC " .. i .. " Servo Out nil") + elseif self.srv_rpm_in_err_status[i] == false then + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "ESC " .. i .. " Servo Out nil") + self.srv_telem_in_err_status[i] = true + end + -- Telemetry data is fresh and valid + else + if self.srv_telem_in_err_status[i] == true then + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "ESC " .. i .. " Telemetry Recovered") + self.srv_telem_in_err_status[i] = false + self.servo_out_nil_counter[i] = 0 + self.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 + -- If the PWM is below the threshold, it is okay for the motor to be stopped + if servo_out < self.SERVO_OUT_THRESHOLD then + self:esc_is_stopped(i) + -- If the PWM has just gone above the threshold, start the warm-up timer + elseif servo_out > self.SERVO_OUT_THRESHOLD and not self.esc_warmup_end_time[i] then + self:esc_is_started(i) + -- If the motor is running, and the warmup timer has expired, check that the motor is spinning + elseif self.esc_warmup_end_time[i] and millis() > self.esc_warmup_end_time[i] then + if servo_out > self.SERVO_OUT_THRESHOLD and esc_rpm < self.ESC_RPM_THRESHOLD then + if self.srv_rpm_in_err_status[i] == false then + cx_msg:send(cx_msg.MAV_SEVERITY.CRITICAL, "ESC " .. i .. " RPM Drop") + self.srv_rpm_in_err_status[i] = true + end + else + if self.srv_rpm_in_err_status[i] == true then + cx_msg:send(cx_msg.MAV_SEVERITY.INFO, "ESC " .. i .. " RPM Recovered") + self.srv_rpm_in_err_status[i] = false + end + end + end + else + self:esc_is_stopped(i) + 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 + self.srv_prv_telem_ms[i] = esc_last_telem_data_ms + end + end +end + +-- Return error messages +function ESC:check_for_errors() + for _, status in ipairs(self.srv_telem_in_err_status) do + if status then + return {"ESC Telemetry Lost"} + end + end + return {} +end + +return ESC diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua new file mode 100644 index 0000000000..622f582dd1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua @@ -0,0 +1,19 @@ +-- MACROS +SCRIPT_NAME = 'CX_BIT' + +local msg = { + -- MAVLink severity level definitions + MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}, +} + +-- wrapper for gcs:send_text(). Helps identify bit messages +function msg:send(severity, txt) + if type(severity) == 'string' then + -- allow just a string to be passed for simple/routine messages + txt = severity + severity = self.MAV_SEVERITY.INFO + end + gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt)) +end + +return msg