From d63cfabc22c2330f21cd242fcc3b91acc4ae9b34 Mon Sep 17 00:00:00 2001
From: Bob Long <robertlong13@gmail.com>
Date: Wed, 25 Sep 2024 19:21:27 +1000
Subject: [PATCH] hwdef: CarbonixCommon: add cx_vtol_info.lua

---
 .../CarbonixCommon/scripts/cx_vtol_info.lua   | 69 +++++++++++++++++++
 .../scripts/cx_vtol_info.lua                  |  1 +
 .../scripts/cx_vtol_info.lua                  |  1 +
 3 files changed, 71 insertions(+)
 create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua
 create mode 120000 libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Ottano/scripts/cx_vtol_info.lua
 create mode 120000 libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Volanti/scripts/cx_vtol_info.lua

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua
new file mode 100644
index 0000000000..ffcd468817
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_vtol_info.lua
@@ -0,0 +1,69 @@
+--[[
+   Send VTOL timer and VTOL state to GCS using named floats
+--]]
+
+local RATE_HZ = 1
+
+local vtol_time_s = 0
+local last_arm_state = false
+
+local last_warning_s = 0
+
+local MAV_SEVERITY_ERROR = 3
+local MAV_SEVERITY_WARNING = 4
+local MAV_SEVERITY_INFO = 6
+
+-- get time in seconds since boot
+local function get_time()
+    return millis():tofloat() * 0.001
+end
+
+local function update()
+    local vtol_state = quadplane:in_vtol_mode()
+    local assist_state = quadplane:in_assisted_flight()
+    local arm_state = arming:is_armed()
+
+    -- Clear timer on transition from disarmed to armed
+    if arm_state and not last_arm_state then
+        vtol_time_s = 0
+    end
+    last_arm_state = arm_state
+
+    -- Increment timer if in VTOL mode
+    if vehicle:get_likely_flying() and arm_state then
+        if vtol_state then
+            vtol_time_s = vtol_time_s + 1 / RATE_HZ
+        -- Increment at half-rate if in assisted mode
+        elseif assist_state then
+            vtol_time_s = vtol_time_s + 1 / (RATE_HZ * 2)
+        end
+    end
+
+    -- Send VTOL state and timer to GCS
+    gcs:send_named_float("VTOLState", vtol_state and 1 or 0)
+    gcs:send_named_float("VTOLTime", vtol_time_s)
+
+    -- Send warning every 10s if in assisted mode
+    if assist_state and (get_time() - last_warning_s) > 10 then
+        gcs:send_text(MAV_SEVERITY_WARNING, "QASSIST")
+        last_warning_s = get_time()
+    end
+end
+
+gcs:send_text(MAV_SEVERITY_INFO, "VTOL Info loaded")
+
+-- 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:send_text(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, math.floor(1000 / RATE_HZ)
+end
+
+-- start running update loop
+return protected_wrapper()
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Ottano/scripts/cx_vtol_info.lua b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Ottano/scripts/cx_vtol_info.lua
new file mode 120000
index 0000000000..5954f416ca
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Ottano/scripts/cx_vtol_info.lua
@@ -0,0 +1 @@
+../../CarbonixCommon/scripts/cx_vtol_info.lua
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Volanti/scripts/cx_vtol_info.lua b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Volanti/scripts/cx_vtol_info.lua
new file mode 120000
index 0000000000..5954f416ca
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-Volanti/scripts/cx_vtol_info.lua
@@ -0,0 +1 @@
+../../CarbonixCommon/scripts/cx_vtol_info.lua
\ No newline at end of file