Update SoarNav.lua

Author: robustini

libraries/AP_Scripting/applets/SoarNav.lua

@@ -1,6 +1,6 @@
 --[[
 SoarNav: an advanced feature for hunting thermals by Marco Robustini.
--- Version 0.9.1 - 2025/07/29
+-- Version 0.9.2 - 2025/07/29
 
 In early versions of SOAR mode in Ardupilot, which I collaborated on with 
 the author, when the glider exited THERMAL mode the heading pointed 
@@ -124,7 +124,7 @@ Script Parameters (SNAV_*)
 
 --[[ SCRIPT INITIALIZATION ]]--
 
-math.randomseed(tonumber(tostring(millis())))
+math.randomseed(tonumber(tostring(millis() or 0)))
 
 -- Enums for GCS message severity and script internal state
 local MAV_SEVERITY = {
@@ -480,18 +480,16 @@ local function calculate_bearing(lat1, lon1, lat2, lon2)
     return (brng + 360) % 360
 end
 
-if not location_offset then
-    function snav_snav_location_offset(lat, lon, brng, dist)
-        local R = 6371000
-        local lat_rad, lon_rad = math.rad(lat), math.rad(lon)
-        local brng_rad = math.rad(brng)
-        local new_lat_rad = math.asin(math.sin(lat_rad) * math.cos(dist / R) + math.cos(lat_rad) * math.sin(dist / R) * math.cos(brng_rad))
-        local new_lon_rad = lon_rad + atan2_safe(math.sin(brng_rad) * math.sin(dist / R) * math.cos(lat_rad), math.cos(dist / R) - math.sin(lat_rad) * math.sin(new_lat_rad))
-        local new_loc = Location()
-        new_loc:lat(math.floor(math.deg(new_lat_rad) * 1e7 + 0.5))
-        new_loc:lng(math.floor(math.deg(new_lon_rad) * 1e7 + 0.5))
-        return new_loc
-    end
+local function location_offset(lat, lon, brng, dist)
+    local R = 6371000
+    local lat_rad, lon_rad = math.rad(lat), math.rad(lon)
+    local brng_rad = math.rad(brng)
+    local new_lat_rad = math.asin(math.sin(lat_rad) * math.cos(dist / R) + math.cos(lat_rad) * math.sin(dist / R) * math.cos(brng_rad))
+    local new_lon_rad = lon_rad + atan2_safe(math.sin(brng_rad) * math.sin(dist / R) * math.cos(lat_rad), math.cos(dist / R) - math.sin(lat_rad) * math.sin(new_lat_rad))
+    local new_loc = Location()
+    new_loc:lat(math.floor(math.deg(new_lat_rad) * 1e7 + 0.5))
+    new_loc:lng(math.floor(math.deg(new_lon_rad) * 1e7 + 0.5))
+    return new_loc
 end
 
 -- Point-in-polygon check algorithm
@@ -663,8 +661,8 @@ local function manage_grid_initialization()
             local max_dist = p_max_dist:get() or 500
             local center_lat, center_lon = active_center:lat()/1e7, active_center:lng()/1e7
             local corner_dist = max_dist * 1.4142
-            local sw_corner_loc = snav_snav_location_offset(center_lat, center_lon, 225, corner_dist)
-            local ne_corner_loc = snav_snav_location_offset(center_lat, center_lon, 45, corner_dist)
+            local sw_corner_loc = location_offset(center_lat, center_lon, 225, corner_dist)
+            local ne_corner_loc = location_offset(center_lat, center_lon, 45, corner_dist)
             SoarNavGlobals.grid_bounds = {
                 min_lat = sw_corner_loc:lat()/1e7, max_lat = ne_corner_loc:lat()/1e7,
                 min_lon = sw_corner_loc:lng()/1e7, max_lon = ne_corner_loc:lng()/1e7
@@ -831,7 +829,7 @@ local function stop_and_record_thermal()
             local base_comp_time = safe_get(p_wind_comp, 60)
             local adaptive_comp_time = base_comp_time * math.max(0.5, math.min(2.0, avg_strength / 1.5))
             local offset_dist = wind_vel * adaptive_comp_time
-            local corrected_loc = snav_snav_location_offset(hotspot_lat, hotspot_lon, upwind_bearing, offset_dist)
+            local corrected_loc = location_offset(hotspot_lat, hotspot_lon, upwind_bearing, offset_dist)
             if corrected_loc then
                 hotspot_lat, hotspot_lon = corrected_loc:lat()/1e7, corrected_loc:lng()/1e7
             end
@@ -983,7 +981,7 @@ end
 
 -- Generates a random waypoint, ensuring it's inside the defined area
 local function generate_target_around_point(center_lat, center_lon, radius_m)
-    local target_loc = snav_snav_location_offset(center_lat, center_lon, math.random() * 360, math.sqrt(math.random()) * radius_m)
+    local target_loc = location_offset(center_lat, center_lon, math.random() * 360, math.sqrt(math.random()) * radius_m)
     if not target_loc then return nil, nil end
     local candidate_lat, candidate_lon = target_loc:lat()/1e7, target_loc:lng()/1e7
     local active_center = get_active_center_location()
@@ -1134,7 +1132,7 @@ local function search_for_new_waypoint()
                             local wind_dir_bearing = (math.deg(wind_heading_rad) + 180 + 360) % 360
                             local drift_factor = math.min(SoarNavConstants.MAX_DRIFT_FACTOR, age_s / SoarNavConstants.MAX_DRIFT_AGE_S)
                             drift_dist = wind_vel * age_s * drift_factor
-                            local drifted_loc = snav_snav_location_offset(center_lat, center_lon, wind_dir_bearing, drift_dist)
+                            local drifted_loc = location_offset(center_lat, center_lon, wind_dir_bearing, drift_dist)
                             if drifted_loc then
                                 center_lat = drifted_loc:lat() / 1e7
                                 center_lon = drifted_loc:lng() / 1e7
@@ -1163,7 +1161,7 @@ local function search_for_new_waypoint()
             local max_dist = safe_get(p_max_dist, 500)
             local rand_dist = math.sqrt(math.random()) * max_dist
             local rand_bearing = math.random() * 360
-            local fallback_loc = snav_snav_location_offset(center_point_lat, center_point_lon, rand_bearing, rand_dist)
+            local fallback_loc = location_offset(center_point_lat, center_point_lon, rand_bearing, rand_dist)
             if fallback_loc then
                 SoarNavGlobals.target_lat, SoarNavGlobals.target_lon = fallback_loc:lat()/1e7, fallback_loc:lng()/1e7
                 new_wp_found = true
@@ -1391,12 +1389,14 @@ update_body = function()
         end
     end
     local rc_val = 0
-    if SoarNavGlobals.detected_soaring_rc_channel then rc_val = rc:get_pwm(SoarNavGlobals.detected_soaring_rc_channel) end
-    local script_switch_high = rc_val and rc_val >= SoarNavConstants.rc_thresh_high
+    if SoarNavGlobals.detected_soaring_rc_channel then
+        rc_val = rc:get_pwm(SoarNavGlobals.detected_soaring_rc_channel) or 0
+    end
+    local script_switch_high = rc_val >= SoarNavConstants.rc_thresh_high
     local home = ahrs:get_home()
     local dist_from_home_3d = ahrs:get_relative_position_NED_home()
     local can_navigate = false
-    if arming:is_armed() and script_switch_high and (current_mode == SoarNavConstants.mode_fbwb or current_mode == SoarNavConstants.mode_cruise) and home and dist_from_home_3d and ahrs:get_yaw() and SoarNavGlobals.grid_initialized then
+    if arming:is_armed() and script_switch_high and (current_mode == SoarNavConstants.mode_fbwb or current_mode == SoarNavConstants.mode_cruise) and home and dist_from_home_3d and ahrs:get_yaw_rad() and SoarNavGlobals.grid_initialized then
         local min_alt = safe_get(p_soar_alt_min, 40)
         local current_alt = -dist_from_home_3d:z()
         if SoarNavGlobals.script_state == SCRIPT_STATE.NAVIGATING or SoarNavGlobals.script_state == SCRIPT_STATE.THERMAL_PAUSE then
@@ -1463,7 +1463,7 @@ update_body = function()
                     log_gcs(MAV_SEVERITY.INFO, 1, string.format("Stuck detected. Repositioning upwind, adaptive distance: %.0fm", repo_dist_m))
                     local wind_heading_rad = atan2_safe(wind_vec:y(), wind_vec:x())
                     local upwind_bearing = (math.deg(wind_heading_rad) + 180 + 360) % 360
-                    local repo_loc = snav_snav_location_offset(current_loc:lat()/1e7, current_loc:lng()/1e7, upwind_bearing, repo_dist_m)
+                    local repo_loc = location_offset(current_loc:lat()/1e7, current_loc:lng()/1e7, upwind_bearing, repo_dist_m)
                     if repo_loc then
                         SoarNavGlobals.target_lat = repo_loc:lat()/1e7
                         SoarNavGlobals.target_lon = repo_loc:lng()/1e7
@@ -1507,7 +1507,7 @@ update_body = function()
             end
             local curr_lat, curr_lon = loc:lat()/1e7, loc:lng()/1e7
             local target_heading = calculate_bearing(curr_lat, curr_lon, SoarNavGlobals.target_lat, SoarNavGlobals.target_lon)
-            local current_heading = math.deg(ahrs:get_yaw())
+            local current_heading = math.deg(ahrs:get_yaw_rad())
             local heading_error = (target_heading - current_heading + 540) % 360 - 180
             local last_grid_update_num = tonumber(tostring(SoarNavGlobals.last_grid_update_ms))
             if last_grid_update_num == 0 or (current_time_num - last_grid_update_num) > SoarNavConstants.grid_update_interval_ms then