ardupilot/libraries/AP_Scripting/applets/advance-wp.lua

--[[----------------------------------------------------------------------------

advance-wp ArduPilot Lua script

Set WAYPT_ADVANCE to an aux function number (e.g. 300).
Set RCx_OPTION to the chosen aux function number (preferably on a momentary switch).

When the RC switch is activated, waypoint index will be advanced to the next waypoint
(wraps to WP1 after the last waypoint).

Mission Planner's Aux Function tab can be used in lieu of dedicating RC channels.

Optionally:
    Set WAYPT_ANNOUNCE to another aux function number (e.g. 301).
    Set WAYPT_ANNOUNCE_S to desired interval (s) between waypoint announcements (0 to disable).
    Set WAYPT_BUZ_ENABLE to 1 to enable buzzer feedback vs waypoint distance.
    Set RCx_OPTION to the chosen aux function number.

    When the announce switch is activated, the current waypoint index, bearing, and distance
    will be broadcast as a GCS message every WAYPT_ANNOUNCE_S seconds (useful when using a
    telemetry link like "Yaapu" where named float values are not always readily displayed).

    If WAYPT_BUZ_ENABLE is set, the buzzer will increase in frequency and pitch as distance
    to the selected waypoint decreases (useful if no telemetry source is readily available).

CAUTION: This script is capable of engaging and disengaging autonomous control
of a vehicle.  Use this script AT YOUR OWN RISK.

-- Yuri -- Apr 2024

LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html
------------------------------------------------------------------------------]]

local RUN_INTERVAL_MS = 100
local PARAM_TABLE_KEY = 193
local PARAM_TABLE_PREFIX = 'WAYPT_'
local MAV_SEVERITY = {
    EMERGENCY = 0,
    ALERT = 1,
    CRITICAL = 2,
    ERROR = 3,
    WARNING = 4,
    NOTICE = 5,
    INFO = 6,
    DEBUG = 7
}

-- borrowed from Rover QuikTune
local function bind_param(name)
    local p = Parameter()
    assert(p:init(name), string.format("Advance WP: Could not find %s parameter", name))
    return p
end

local function bind_add_param(name, idx, default_value)
    assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value),
        string.format('Could not add param %s', name))
    return bind_param(PARAM_TABLE_PREFIX .. name)
end

local function get_wp_location(item)
    local loc = Location()
    loc:lat(item:x())
    loc:lng(item:y())
    loc:alt(math.floor(item:z() * 100))
    return loc
end

local function get_pitch_by_distance(distance)
    local max_distance = 300
    local min_distance = 0.01
    local total_notes = 73

    distance = math.max(min_distance, math.min(distance, max_distance))
    local scale_factor = 105                                    -- scale factor adjusted for a good spread over the distance range
    local log_ratio = math.log(max_distance / distance)
    local max_log_ratio = math.log(max_distance / min_distance) -- max possible value of log_ratio
    local log_distance_scaled = log_ratio / max_log_ratio * total_notes * scale_factor / 100
    local note_index = math.min(math.floor(log_distance_scaled), total_notes - 1)

    return 'N' .. math.max(1, note_index)
end

local function get_buzz_interval(distance)
    local max_distance = 100
    local min_distance = 0.01
    local max_interval = 2000
    local min_interval = 250

    distance = math.max(min_distance, math.min(distance, max_distance))
    local interval = max_interval - (max_interval - min_interval) * ((max_distance - distance) / max_distance)
    return math.floor(interval)
end

assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'Advance WP: Could not add param table')

local WAYPT_ADVANCE = bind_add_param('ADVANCE', 1, 300)
local WAYPT_ANNOUNCE = bind_add_param('ANNOUNCE', 2, 301)
local WAYPT_ANNOUNCE_S = bind_add_param('ANNOUNCE_S', 3, 0)
local WAYPT_BUZ_ENABLE = bind_add_param('BUZ_ENABLE', 4, 1)

local last_advance_sw_pos = -1
local last_announce_ms = uint32_t(0)
local last_buzz_ms = uint32_t(0)
function update()
    ---- WAYPT_ADVANCE ----
    local advance_opt = WAYPT_ADVANCE:get()
    if not advance_opt then return update, RUN_INTERVAL_MS end

    local adv_sw_pos = rc:get_aux_cached(advance_opt)
    if not adv_sw_pos then return update, RUN_INTERVAL_MS end

    local num_commands = mission:num_commands()
    if num_commands < 1 then return update, RUN_INTERVAL_MS end

    if adv_sw_pos > 0 and adv_sw_pos ~= last_advance_sw_pos then
        local nav_index = mission:get_current_nav_index()
        local new_index = (nav_index + 1) % mission:num_commands()
        mission:set_current_cmd(new_index)
        gcs:send_text(MAV_SEVERITY.NOTICE, ('Advance WP -> %d'):format(mission:get_current_nav_index()))
    end
    last_advance_sw_pos = adv_sw_pos or 0

    ---- WAYPT_ANNOUNCE ----
    local announce_s = WAYPT_ANNOUNCE_S:get()
    if not announce_s then return update, RUN_INTERVAL_MS end
    if announce_s <= 0 then return update, RUN_INTERVAL_MS end

    local announce_opt = WAYPT_ANNOUNCE:get()
    if not announce_opt then return update, RUN_INTERVAL_MS end

    local ann_sw_pos = rc:get_aux_cached(announce_opt)
    if not ann_sw_pos then return update, RUN_INTERVAL_MS end

    local now = millis()

    if ann_sw_pos > 0 then
        -- to work when mission is inactive, need to convert current nav item to location
        local nav_index = mission:get_current_nav_index()
        local item = mission:get_item(nav_index)
        local wp_loc = get_wp_location(item)
        local cur_loc = ahrs:get_location()
        if cur_loc then
            local bearing = math.deg(cur_loc:get_bearing(wp_loc))
            local distance = cur_loc:get_distance(wp_loc)

            local buzz_enable = WAYPT_BUZ_ENABLE:get()
            if buzz_enable and buzz_enable > 0 and now - last_buzz_ms > get_buzz_interval(distance) then
                notify:play_tune('MFT240MSL8' .. get_pitch_by_distance(distance))
                last_buzz_ms = now
            end

            if now - last_announce_ms > announce_s * 1000 then
                gcs:send_text(MAV_SEVERITY.NOTICE, ('WP %d: %03.0f° / %.3fm'):format(nav_index, bearing, distance))
                last_announce_ms = now
            end
        elseif now - last_announce_ms > announce_s * 1000 then
            gcs:send_text(MAV_SEVERITY.WARNING, 'Advance WP: Invalid AHRS location')
            last_announce_ms = now
        end
    end

    return update, RUN_INTERVAL_MS
end

gcs:send_text(MAV_SEVERITY.INFO, 'Advance WP Script Active')

return update, RUN_INTERVAL_MS