BRGR scripts merge with master

.gitignore

@@ -1,3 +1,4 @@
+/ArduPlane/ap-*.core
 /.lock-waf*
 /.waf*
 /Tools/autotest/aircraft/Rascal/reset.xml

README.md

@@ -18,7 +18,7 @@
 
 [![Autotest Status](https://autotest.ardupilot.org/autotest-badge.svg)](https://autotest.ardupilot.org/)
 
-ArduPilot is the most advanced, full-featured, and reliable open source autopilot software available.
+ArduPilot is the most advanced, full-featured, and reliable open source autopilot software availabll
 It has been under development since 2010 by a diverse team of professional engineers, computer scientists, and community contributors.
 Our autopilot software is capable of controlling almost any vehicle system imaginable, from conventional airplanes, quad planes, multi-rotors, and helicopters to rovers, boats, balance bots, and even submarines.
 It is continually being expanded to provide support for new emerging vehicle types.

ROMFS_custom/scripts/auto_armer.lua

@@ -0,0 +1,28 @@
+--script to force arm on startup
+local battery_instance = 0
+local voltage_threshold = 7
+local low_power = 25 -- mw value
+
+function update()
+    if not arming:is_armed() then
+        arming:arm()
+    end
+    local voltage = battery:voltage(battery_instance)
+    if voltage == nil then
+        gcs:send_text(6, "No battery data!")
+    else 
+        gcs:send_text(6, string.format("Battery: %.2f V", voltage))
+        if voltage < voltage_threshold then
+            param:set("VTX_POWER", low_power)
+            local power = param:get("VTX_POWER")
+            gcs:send_text(6, "Battery low -> Current VTX_POWER: " .. tostring(power))
+        else
+            gcs:send_text(6, "Battery healthy -> VTX power normal")
+            local power = param:get("VTX_POWER")
+            gcs:send_text(6, "Current VTX_POWER: " .. tostring(power))
+        end
+    end
+    return update, 1000
+end
+
+return update, 1000

ROMFS_custom/scripts/geofence.lua

@@ -0,0 +1,71 @@
+-- Define safe areas as polygons to perform balloon drop. 
+local safe_zones = {
+    -- Can define multiple polygons over ocean areas. 
+    {
+        {lat = 39.758170, lng = -74.065188}, -- (39.758170, -74.065188)
+        {lat = 40.490719, lng = -73.917797}, -- (40.490719, -73.917797)
+        {lat = 40.678260, lng = -72.799620}, -- (40.678260, -72.799620)
+        {lat = 39.684256, lng = -72.799620} --(39.684256, -72.799620)
+    }
+}
+
+function point_in_polygon(lat, lng, polygon)
+    local inside = false
+    local p1x, p1y = polygon[1].lat, polygon[1].lng
+
+    for i = 1, #polygon do
+        local p2x, p2y = polygon[i].lat, polygon[i].lng
+        if ((p2y > lng) ~= (p1y > lng)) and 
+           (lat < (p1x - p2x) * (lng - p2y) / (p1y - p2y) + p2x) then
+            inside = not inside
+        end
+        p1x, p1y = p2x, p2y
+    end
+
+    return inside
+end
+
+function is_over_safe_zone()
+    local loc = ahrs:get_position()
+    if loc == nil then -- Can't determine position
+        return false  
+    end
+
+    local lat = loc:lat() / 1e7  -- Convert from integer degrees
+    local lng = loc:lng() / 1e7
+
+    for _, zone in ipairs(safe_zones) do
+        if point_in_polygon(lat, lng, zone) then
+            return true
+        end
+    end
+
+    return false
+end
+
+-- Script to wait for safe zone using ArduPilot script structure
+
+function update()
+    if not is_over_safe_zone() then
+        -- Optional: Print current position for debugging
+        local loc = ahrs:get_position()
+        if loc then
+            local lat = loc:lat() / 1e7
+            local lng = loc:lng() / 1e7
+            gcs:send_text(6, string.format("Current position: %.6f, %.6f - Not over safe zone", lat, lng))
+        else
+            gcs:send_text(4, "Warning: Unable to get current position")
+        end
+
+        -- Continue checking - return to update function in 1000ms
+        return update, 1000
+    else
+        gcs:send_text(0, "Now over safe zone! Ready for balloon drop.")
+        -- Could add balloon drop logic here, or return nil to stop script
+        return update, 1000  -- Keep running or change to nil to stop
+    end
+end
+
+-- Initial message and start the loop
+--gcs:send_text(0, "Waiting for safe zone...")
+--return update, 1000

ROMFS_custom/scripts/grid_fin_mixing.lua

@@ -0,0 +1,134 @@
+function update()
+    if arming:is_armed() then
+        --gcs:send_text(6, "ARMED LUA")
+        -- get aileron, rudder, elevator PWM
+        local a_pwm = SRV_Channels:get_output_pwm(4)
+        local r_pwm = SRV_Channels:get_output_pwm(21)
+        local e_pwm = SRV_Channels:get_output_pwm(19)
+
+        if not a_pwm or not r_pwm or not e_pwm then
+            gcs:send_text(6, "no pwm!")
+            return update, 10
+        end
+
+        -- inputs must be between 1000 and 2000
+        a_pwm = math.min(math.max(1000, a_pwm), 2000)
+        r_pwm = math.min(math.max(1000, r_pwm), 2000)
+        e_pwm = math.min(math.max(1000, e_pwm), 2000)
+
+
+
+        -- normalize
+        local a_norm = (a_pwm - 1500) / 500.0
+        local r_norm = (r_pwm - 1500) / 500.0
+        local e_norm = (e_pwm - 1500) / 500.0
+
+        local yaw_adjustment_factor = 0.5
+
+        local elevator_trim = 0.2
+        e_norm = e_norm + elevator_trim
+        -- gridfin deflections
+        local deflection_top  = -a_norm * 0.2 + r_norm * 0.2
+        local deflection_star = -a_norm * 0.2 + r_norm * yaw_adjustment_factor * 0.2 - e_norm * 0.5
+        local deflection_port = -a_norm * 0.2 + r_norm * yaw_adjustment_factor * 0.2 + e_norm * 0.5
+
+
+
+
+        -- get velocity
+        local V_ned = ahrs:get_velocity_NED()
+        local V = nil
+        local V_x, V_y, V_z = nil, nil, nil
+
+        if V_ned ~= nil then
+            local V_wind = ahrs:wind_estimate()
+            local V_air = nil
+
+            if V_wind == nil then
+                --V_wind = Vector3f(0,0,0) doesn't work don't do this
+                V_air = V_ned
+            else
+                V_air = V_ned - V_wind
+            end
+
+            local V_body = ahrs:earth_to_body(V_air)
+
+            V_x = V_body:x()  -- forward +x
+            V_y = V_body:y()  -- right +y
+            V_z = V_body:z()  -- down +z
+            V = V_body:length()
+
+        end
+
+        -- limit the deflections
+
+
+
+        --if V ~= nil and V > 3 then
+        --    -- calculate AOA and beta (sideslip)
+        --    AOA = math.atan(V_z, V_x)
+        --    beta = math.asin(V_y/V)
+        --   
+            -- limit fin pwm:
+        --    deflection_top=limit_extended(deflection_top, 0, AOA, beta)
+        --    deflection_star=limit_extended(deflection_star,  2*math.pi/3, AOA, beta)
+        --    deflection_port=limit_extended(deflection_port, -2*math.pi/3, AOA, beta)
+        --else
+        deflection_top=limit_failover(deflection_top)
+        deflection_star=limit_failover(deflection_star)
+        deflection_port=limit_failover(deflection_port)
+        --end
+
+        if millis() - last_time_gfm_ms > 1000 then
+            last_time_gfm_ms = millis()
+
+            gcs:send_text(6, "Top D: " .. deflection_top)
+            gcs:send_text(6, "Velocity: ".. V)
+            --gcs:send_text(6, "AOA = " .. math.deg(AOA))
+            --gcs:send_text(6, "beta = ".. math.deg(beta))
+        end
+
+        -- deflections in PWM
+        local g_top_pwm  = math.floor(deflection_top  * 500 + 1500)
+        local g_star_pwm = math.floor(deflection_star * 500 + 1500)
+        local g_port_pwm = math.floor(deflection_port * 500 + 1500)
+
+        -- set gridfin pwms
+
+        if vehicle:get_mode() == 0 then
+            SRV_Channels:set_output_pwm(94, 1500)
+            SRV_Channels:set_output_pwm(95, 1450)
+            SRV_Channels:set_output_pwm(96, 1550)
+        end
+
+        if vehicle:get_mode() ~= 0 then 
+            SRV_Channels:set_output_pwm(94, g_top_pwm)
+            SRV_Channels:set_output_pwm(95, g_star_pwm)
+            SRV_Channels:set_output_pwm(96, g_port_pwm)
+        end
+    end
+    return update, 10
+end
+
+function limit_extended(fin_deflection, fin_angle, AOA, beta)
+    local position_correction = AOA * math.sin(fin_angle) - beta * math.cos(fin_angle)
+    local effective_aoa = fin_deflection + position_correction
+    --gcs:send_text(6, string.format("eff = %.2f rad", effective_aoa))
+
+    local limit = math.rad(20)
+    local upper_limit = limit + position_correction
+    local lower_limit = limit - position_correction
+
+    return math.min(math.max(lower_limit, fin_deflection), upper_limit)
+
+end
+
+function limit_failover(fin_deflection)
+    local limit = math.rad(20)
+    return math.min(math.max(-limit, fin_deflection), limit)
+end 
+
+
+last_time_gfm_ms = millis()
+
+return update, 10