Skip to content

Commit 533baba

Browse files
yuri-rageyuri-rage
committed
AP_Scripting: add advance-wp.lua
1 parent 890a831 commit 533baba

File tree

2 files changed

+202
-0
lines changed

2 files changed

+202
-0
lines changed

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

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
--[[----------------------------------------------------------------------------
2+
3+
advance-wp ArduPilot Lua script
4+
5+
Set WAYPT_ADVANCE to an aux function number (e.g. 300).
6+
Set RCx_OPTION to the chosen aux function number (preferably on a momentary switch).
7+
8+
When the RC switch is activated, waypoint index will be advanced to the next waypoint
9+
(wraps to WP1 after the last waypoint).
10+
11+
Mission Planner's Aux Function tab can be used in lieu of dedicating RC channels.
12+
13+
Optionally:
14+
Set WAYPT_ANNOUNCE to another aux function number (e.g. 301).
15+
Set WAYPT_ANNOUNCE_S to desired interval (s) between waypoint announcements (0 to disable).
16+
Set WAYPT_BUZ_ENABLE to 1 to enable buzzer feedback vs waypoint distance.
17+
Set RCx_OPTION to the chosen aux function number.
18+
19+
When the announce switch is activated, the current waypoint index, bearing, and distance
20+
will be broadcast as a GCS message every WAYPT_ANNOUNCE_S seconds (useful when using a
21+
telemetry link like "Yaapu" where named float values are not always readily displayed).
22+
23+
If WAYPT_BUZ_ENABLE is set, the buzzer will increase in frequency and pitch as distance
24+
to the selected waypoint decreases (useful if no telemetry source is readily available).
25+
26+
CAUTION: This script is capable of engaging and disengaging autonomous control
27+
of a vehicle. Use this script AT YOUR OWN RISK.
28+
29+
-- Yuri -- Apr 2024
30+
31+
LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html
32+
------------------------------------------------------------------------------]]
33+
34+
local RUN_INTERVAL_MS = 100
35+
local PARAM_TABLE_KEY = 193
36+
local PARAM_TABLE_PREFIX = 'WAYPT_'
37+
local MAV_SEVERITY = {
38+
EMERGENCY = 0,
39+
ALERT = 1,
40+
CRITICAL = 2,
41+
ERROR = 3,
42+
WARNING = 4,
43+
NOTICE = 5,
44+
INFO = 6,
45+
DEBUG = 7
46+
}
47+
48+
-- borrowed from Rover QuikTune
49+
local function bind_param(name)
50+
local p = Parameter()
51+
assert(p:init(name), string.format("Advance WP: Could not find %s parameter", name))
52+
return p
53+
end
54+
55+
local function bind_add_param(name, idx, default_value)
56+
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value),
57+
string.format('Could not add param %s', name))
58+
return bind_param(PARAM_TABLE_PREFIX .. name)
59+
end
60+
61+
local function get_wp_location(item)
62+
local loc = Location()
63+
loc:lat(item:x())
64+
loc:lng(item:y())
65+
loc:alt(math.floor(item:z() * 100))
66+
return loc
67+
end
68+
69+
local function get_pitch_by_distance(distance)
70+
local max_distance = 300
71+
local min_distance = 0.01
72+
local total_notes = 73
73+
74+
distance = math.max(min_distance, math.min(distance, max_distance))
75+
local scale_factor = 105 -- scale factor adjusted for a good spread over the distance range
76+
local log_ratio = math.log(max_distance / distance)
77+
local max_log_ratio = math.log(max_distance / min_distance) -- max possible value of log_ratio
78+
local log_distance_scaled = log_ratio / max_log_ratio * total_notes * scale_factor / 100
79+
local note_index = math.min(math.floor(log_distance_scaled), total_notes - 1)
80+
81+
return 'N' .. math.max(1, note_index)
82+
end
83+
84+
local function get_buzz_interval(distance)
85+
local max_distance = 100
86+
local min_distance = 0.01
87+
local max_interval = 2000
88+
local min_interval = 250
89+
90+
distance = math.max(min_distance, math.min(distance, max_distance))
91+
local interval = max_interval - (max_interval - min_interval) * ((max_distance - distance) / max_distance)
92+
return math.floor(interval)
93+
end
94+
95+
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'Advance WP: Could not add param table')
96+
97+
local WAYPT_ADVANCE = bind_add_param('ADVANCE', 1, 300)
98+
local WAYPT_ANNOUNCE = bind_add_param('ANNOUNCE', 2, 301)
99+
local WAYPT_ANNOUNCE_S = bind_add_param('ANNOUNCE_S', 3, 0)
100+
local WAYPT_BUZ_ENABLE = bind_add_param('BUZ_ENABLE', 4, 1)
101+
102+
local last_advance_sw_pos = -1
103+
local last_announce_ms = uint32_t(0)
104+
local last_buzz_ms = uint32_t(0)
105+
function update()
106+
---- WAYPT_ADVANCE ----
107+
local advance_opt = WAYPT_ADVANCE:get()
108+
if not advance_opt then return update, RUN_INTERVAL_MS end
109+
110+
local adv_sw_pos = rc:get_aux_cached(advance_opt)
111+
if not adv_sw_pos then return update, RUN_INTERVAL_MS end
112+
113+
local num_commands = mission:num_commands()
114+
if num_commands < 1 then return update, RUN_INTERVAL_MS end
115+
116+
if adv_sw_pos > 0 and adv_sw_pos ~= last_advance_sw_pos then
117+
local nav_index = mission:get_current_nav_index()
118+
local new_index = (nav_index + 1) % mission:num_commands()
119+
mission:set_current_cmd(new_index)
120+
gcs:send_text(MAV_SEVERITY.NOTICE, ('Advance WP -> %d'):format(mission:get_current_nav_index()))
121+
end
122+
last_advance_sw_pos = adv_sw_pos or 0
123+
124+
---- WAYPT_ANNOUNCE ----
125+
local announce_s = WAYPT_ANNOUNCE_S:get()
126+
if not announce_s then return update, RUN_INTERVAL_MS end
127+
if announce_s <= 0 then return update, RUN_INTERVAL_MS end
128+
129+
local announce_opt = WAYPT_ANNOUNCE:get()
130+
if not announce_opt then return update, RUN_INTERVAL_MS end
131+
132+
local ann_sw_pos = rc:get_aux_cached(announce_opt)
133+
if not ann_sw_pos then return update, RUN_INTERVAL_MS end
134+
135+
local now = millis()
136+
137+
if ann_sw_pos > 0 then
138+
-- to work when mission is inactive, need to convert current nav item to location
139+
local nav_index = mission:get_current_nav_index()
140+
local item = mission:get_item(nav_index)
141+
local wp_loc = get_wp_location(item)
142+
local cur_loc = ahrs:get_location()
143+
if cur_loc then
144+
local bearing = math.deg(cur_loc:get_bearing(wp_loc))
145+
local distance = cur_loc:get_distance(wp_loc)
146+
147+
local buzz_enable = WAYPT_BUZ_ENABLE:get()
148+
if buzz_enable and buzz_enable > 0 and now - last_buzz_ms > get_buzz_interval(distance) then
149+
notify:play_tune('MFT240MSL8' .. get_pitch_by_distance(distance))
150+
last_buzz_ms = now
151+
end
152+
153+
if now - last_announce_ms > announce_s * 1000 then
154+
gcs:send_text(MAV_SEVERITY.NOTICE, ('WP %d: %03.0f° / %.3fm'):format(nav_index, bearing, distance))
155+
last_announce_ms = now
156+
end
157+
elseif now - last_announce_ms > announce_s * 1000 then
158+
gcs:send_text(MAV_SEVERITY.WARNING, 'Advance WP: Invalid AHRS location')
159+
last_announce_ms = now
160+
end
161+
end
162+
163+
return update, RUN_INTERVAL_MS
164+
end
165+
166+
gcs:send_text(MAV_SEVERITY.INFO, 'Advance WP Script Active')
167+
168+
return update, RUN_INTERVAL_MS

‎libraries/AP_Scripting/applets/advance-wp.md

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
# Advance Waypoint
2+
3+
Advance Waypoint (`advance-wp.lua`) allows for advancing the current mission waypoint via an RC switch. When the RC switch state is high, the mission waypoint is advanced to the next waypoint (wraps back to WP 1 if the end of the mission is reached).
4+
5+
## How to Use
6+
7+
Install this script in the autopilot's SD card in the `APM/scripts` directory. Set SCR_ENABLE to 1 and reboot the autopilot.
8+
9+
* Set WAYPT_ADVANCE to an available aux function (300 by default).
10+
* Set RCx_OPTION to the chosen aux function number. Preferably, set this to a channel assigned to a momentary switch.
11+
12+
Mission Planner's Aux Function tab can be used in lieu of dedicating an RC channel.
13+
14+
## Additional Features
15+
16+
If Yaapu telemetry (or similar) is in use on the RC transmitter, it may be useful to display bearing and distance to the currently selected waypoint in the messages view, regardless of flight mode or arming state. To enable this:
17+
18+
* Set WAYPT_ANNOUNCE to another available aux function (301 by default).
19+
* Set RCx_OPTION to the chosen aux function.
20+
* Set WAYPT_ANNOUNCE_S to desired interval (in seconds) between waypoint announcements (0 disables the feature).
21+
22+
As above, Mission Planner's Aux Function tab can be used in lieu of dedicating an RC channel.
23+
24+
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.
25+
26+
Additionally, there is an audio feedback feature:
27+
28+
* Set WAYPT_BUZ_ENABLE to 1 to enable buzzer feedback (0 to disable).
29+
30+
If WAYPT_BUZ_ENABLE is set, the buzzer will beep when the announce switch is activated, increasing in frequency and pitch as distance to the selected waypoint decreases (useful as a rangefinder for the selected waypoint if no telemetry source is readily available).
31+
32+
### Author's Note
33+
34+
I used this script to create a survey "prism pole" of sorts out of a spare autopilot, RTK GPS module/antenna, RC receiver, and telemetry radio. Using the existing SaveWP feature along with this script, I can save waypoints and subsequently relocate them precisely with the additional features above.

0 commit comments

Comments
 (0)