I am trying to write a lua script for changing altitude. For instance, if drone is 20m alt, I want it to go to 10m, be there for two seconds and then again go to previous altitude.
Is it even possible to do with lua script or I have to consider raspberry pi + mavutil/dronekit?
Sorry for formatting, writing on my mobile phone
This is my code I have written so far:
-- ArduPilot Lua Script to descend to 5m and return to previous altitude
-- This script assumes the use of GUIDED mode and that the vehicle is armed
local target_alt = 5.0 -- target altitude in meters (AGL)
local current_stage = 0
local original_alt = 0
local reached_target = false
function update()
-- Only proceed if vehicle is in GUIDED mode and armed
if not arming:is_armed() or vehicle:get_mode() ~= 4 then
gcs:send_text(6, "Waiting for GUIDED mode and armed")
return update, 1000
end
local alt = ahrs:altitude_relative() -- Get altitude above home (AGL) in meters
if current_stage == 0 then
original_alt = alt
gcs:send_text(6, string.format("Original Altitude: %.2f m", original_alt))
vehicle:set_target_altitude(target_alt)
gcs:send_text(6, string.format("Descending to %.2f m", target_alt))
current_stage = 1
return update, 1000
elseif current_stage == 1 then
if math.abs(alt - target_alt) < 0.5 then
gcs:send_text(6, "Reached 5m, returning to original altitude")
vehicle:set_target_altitude(original_alt)
current_stage = 2
end
return update, 1000
elseif current_stage == 2 then
if math.abs(alt - original_alt) < 0.5 then
gcs:send_text(6, "Returned to original altitude")
current_stage = 3
end
return update, 1000
else
-- Finished, do nothing
return update, 3000
end
end
return update()
Could you please at least provide me with small example how it should be done?
I have found this example however it needs to be modified, for instance, it should start deploy mission only when user activates appropriate AUX. Further more , I am not sure when I should activate GUIDED mode.
How did you come up with the code in your first example? I do not see a set_target_altitude function in the codebase.
A couple of ways possibly:
Look into the function set_target_pos_NED, if the vehicle is in Guided mode it sets a target position with x, y, and z position data.
I don’t think this next way is really recommended, but you could write a Lua script to do an RC override for your throttle channel and read the altitude from either your baro or rangefinder in order to drive the throttle.
I can’t use GUIDED because I am not allowed to use GPS. I use GUIDED_NO_GPS mode. vehicle:set_target_altitude does not climb. I used another function to add throttle. Also, I dont want to override RC since you need constantly override it
If you are able and willing to put work into exposing ArduPilot code for use in Lua scripting AND you have a range finder, then this may work for you. It does not require GPS nor does it require RC override…
local home_alt = rangefinder:distance_orient(rangefinder.ROTATION_PITCH_270)
local current_alt = home_alt
local target_alt = current_alt + 5
function update()
while (current_alt ~= target_alt) do // probably want to add some tolerance here like maybe be within 5% of your target altitude
// Get the vehicle's motor hover value
local throttle_hover = motors:get_throttle_hover()
// Below target altitude - need to increase throttle
if (current_alt < target_alt) then
// Increase motor throttle output by 5%
// Definitely want to add some logic here to keep this from getting called every cycle
attitude_control:set_throttle_out(throttle_hover * 1.05)
elseif (current_alt > target_alt) then
attitude_control:set_throttle_out(throttle_hover * 0.95)
end
current_alt = rangefinder:distance_orient(rangefinder.ROTATION_PITCH_270)
end
// Then add some more stuff here about going back to your home_alt
return update, 3000
end
That is just some rough code that needs some extra work. First step is exposing those functions for Lua scripting. Then you need a lot of checks and balances to make sure your throttle doesn’t just skyrocket and your vehicle flies into space, or throttle drops and your vehicle just drops into the ground.
I have tested/used the throttle output code like this before, but in the codebase and recompiling, and adjusting the throttle output does work. Alternatively you could use a barometer but depending on how accurate your altitude needs to be a range finder is going to be better.
It just takes some design and planning to make it work how you want.
woow, thank you, I have already finished that script and it works, the most weak place in my code was climbing because I have used vehicle:set_target_angle_and_climbrate was not aware that there is global motors. Also is attitude_control a global variable?
Take a look here: bindings.desc. Anything in there is exposed for use in Lua.
If you want to expose more, then take a look here: Lua Scripts
Specifically,
How to Add New Bindings
To give Lua scripts access to more features of ArduPilot the API can be extended by creating new bindings. The process is as follows:
Find the method or function you would like to expose to Lua. For example if you wanted to expose an additional feature of AHRS you would first find the method within libraries/AP_AHRS/AP_AHRS.h. This can be an already existing method (function) or a method (function) newly added to the code.
Thank you, I have considered this example, however they use ahrs, NED and Vector in GUIDED mode which is not suitable for me since I am not allowed to use GPS