WIP

SW-162
CarbonixUAV · Apr 26, 2024 · 520eab6 · 520eab6
1 parent 2b795b4
commit 520eab6
Showing 1 changed file with 44 additions and 20 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/cx_built_in_test.lua b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/cx_built_in_test.lua
@@ -29,11 +29,17 @@ local OTTANO = 2
 -- Engine Types
 local HIRTH_EFI_TYPE = 8
 
+local ESC_WARMUP_TIME = 3000
+local SERVO_OUT_THRESHOLD = 1010
+local ESC_RPM_THRESHOLD = 10
 -- ******************* Variables *******************
 
 local aircraft_type = VOLANTI
 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},
@@ -51,12 +57,6 @@ local srv_number = {
 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 PARAM_TABLE_KEY = 101
-local SERVO_OUT_THRESHOLD = 1010
-local ESC_RPM_THRESHOLD = 10
 
 -- ******************* Objects *******************
 
@@ -121,6 +121,18 @@ local function arming_check_init()
     return true
 end
 
+-- Call this function whenever a motor starts running
+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
+function esc_is_stopped(i)
+    -- Clear the warm-up end time for this ESC
+    esc_warmup_end_time[i] = nil
+end
+
 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()
@@ -134,27 +146,39 @@ local function esc_check_loop()
                 srv_prv_telem_ms[i] = esc_last_telem_data_ms
             end
         else
+            if srv_telem_in_err_status[i] == true then
+                gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered")
+                srv_telem_in_err_status[i] = false
+            end
             if arming:is_armed() then
                 local esc_rpm = esc_telem:get_rpm(i-1)
                 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")
-                            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
+                    -- If the PWM is below the threshold, consider it a stopped motor situation
+                    if servo_out < SERVO_OUT_THRESHOLD then
+                        esc_is_stopped(i)
+                    -- If the PWM is above the threshold consider it start the motor
+                    elseif servo_out > SERVO_OUT_THRESHOLD and esc_warmup_end_time[i] == nil then
+                        esc_is_started(i)
+                    -- If the motor is running and the PWM is above the threshold, check the RPM
+                    elseif esc_warmup_end_time[i] ~= nil 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(MSG_NORMAL, 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(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " RPM Recovered")
+                                srv_rpm_in_err_status[i] = false
+                            end
                         end
                     end
+                else
+                    gcs_msg(MSG_NORMAL, MAV_SEVERITY_CRITICAL, "ESC " .. i .. " Null Rcout or RPM")
+                    srv_telem_in_err_status[i] = true
                 end
             end
-            if srv_telem_in_err_status[i] == true then
-                gcs_msg(MSG_NORMAL, MAV_SEVERITY_INFO, "ESC " .. i .. " Telemetry Recovered")
-                srv_telem_in_err_status[i] = false
-            end
             srv_prv_telem_ms[i] = esc_last_telem_data_ms
         end
     end
@@ -193,7 +217,7 @@ local function protected_wrapper()
         -- down a bit so we don't flood the console with errors
         return protected_wrapper, 1000
     end
-    return protected_wrapper, 1000
+    return protected_wrapper, 200
 end
 
 local function script_exit()