I’m new to Lua scripting and I’m trying to create a script that adds a waypoint at the current position when the velocity (measured by the AHRS) exceeds 4 m/s. This is an example I’m working on in SITL.
The script should save the current position as a waypoint, and then initiate Return to Launch (RTL). After RTL is triggered, if I switch the vehicle back to auto mode, it should return to the saved waypoint.
Here’s my Lua script:
—@diagnostic disable: need-check-nil
local WAYPOINT = 16
function save_current_wp()
local wp = mission:get_item(0)
local position = ahrs:get_position()
if (not wp) or (not position) then
gcs:send_text(0, "Unable to save waypoint: No home point or position data!")
return false
end
-- Create a new waypoint at the current location
wp:command(WAYPOINT)
wp:x(position:lat())
wp:y(position:lng())
wp:z(position:alt()) -- Use current altitude
-- Add the new waypoint to the end of the mission
local new_index = mission:num_commands()
mission:set_item(new_index, wp)
gcs:send_text(0, "Waypoint saved at current location!")
return true
end
function update()
if arming:is_armed() and vehicle:get_mode() == 10 then
local ground_speed_vector = ahrs:groundspeed_vector()
local ground_speed = math.sqrt(ground_speed_vector:x()^2 + ground_speed_vector:y()^2)
gcs:send_text(0, string.format("Ground Speed: %.2f m/s", ground_speed))
if ground_speed > 4 then
gcs:send_text(0, "Ground speed > 4, RTL nha")
gcs:send_text(0, "Tat dia xoay")
gcs:send_text(0, "Dong gate")
if save_current_wp() then
-- Initiate RTL
gcs:send_text(0, "Initiating RTL")
vehicle:set_mode(11) -- RTL mode
else
gcs:send_text(0, "Failed to save waypoint. RTL not initiated.")
vehicle:set_mode(4) -- Hold
end
end
end
return update, 1000
end
return update()
I’ve read some Lua discussions where @Yuri_Rage helped others with similar issues, and I hope you can help me find a solution too, as I haven’t been able to solve it yet.
You don’t need waypoints to simply set a different destination point for RTL. It is sufficient and easier to move the home point (which is normally the destination point for RTL):
In Arduplane I do it like this:
local wp_actual = ahrs:get_location() -- current position
wp_actual:change_alt_frame(3) -- above terrain
wp_actual:alt(0) -- set height to 0
ahrs:set_home(wp_actual)
Thank you so much for your reply, Rolf. I appreciate the insight!
In my case, the rover needs to return home for resupply. I’m working on an RC vehicle that spreads minerals along a mission route. When it runs out of minerals, it should return to the home base for a refill, then come back to the exact spot where it ran out and continue spreading. This way, the operation can proceed smoothly without missing any sections.
However, I noticed an issue: if the rover passes waypoint 1 and is on the way to waypoint 2, and it runs out of minerals, it triggers RTL but then heads straight to waypoint 2 after resupply, rather than returning to the exact spot where it stopped, as shown in the figure I attached. Would your approach still work in this case, or would adding a waypoint at the stopping point be more appropriate?
Thank you so much, EosBandi. For the past 4 hours, I’ve been working through Yuri’s code, trying to understand it and making a few modifications. I removed the “should ensure takeoff is complete” part since I’m working with a rover, and I also removed the battery failsafe condition.
My purpose is to get ground velocity >4 so then it will save waypoint at current location and RTL. but when the velocity > 4 it shows “Auto mission changed, restarted command” then go in circle.
Here the lua code that I have modified: test.lua (5.0 KB)
And here is the figure and message that it sent and also what it sent through message:
I feel like I’m close to getting it right, but I’d really appreciate any help or advice if you have the time. Thank you so much to everyone for the support.
Hmm, didn’t you forgot something from the two actions above ?
You still running the mission in auto mode when the script rewrites the script. There is no RTL command anywhere…
