ardupilot/libraries/AP_Scripting/examples/copter_pingpong.md

Drone Ping-Pong Code

This script makes the drone go forward and backward a defined distance and number of times.

Stages

• 0: Change to Guided mode
• 1: Takeoff to the height defined by takeoff_alt
• 2: Wait until reaching the takeoff altitude
• 3: Go forward to the defined distance
• 4: Go back to the initial position
• 5: Change to Land mode

Variables:

• takeoff_alt: Takeoff height (m)
• copter_guided_mode_num: Guided mode number
• copter_land_mode_num: Land mode number
• stage: current stage
• count: Number of times the drone has gone forward
• max_count: Maximum number of times the drone should go forward
• ping_pong_distance: Distance up to which the drone should go forward (m)
• vel: Drone velocity (m/s)

Understand the code:

First, there is a comment explaining what the code is about and how it works. Then, the local variables that will be used are declared. The copter_guided_mode_num and copter_land_mode_num are standard numbers, defined by ArduPilot, for the Guided and Land flight modes, respectively. The other variables are user-defined settings according to the desired behavior, as indicated in the comments in front of each one.

-- This script makes the drone go forward and backward at a defined distance and number of times.
-- The stages are:
-- 0) Change to Guided mode
-- 1) Takeoff to the height defined by takeoff_alt
-- 2) Wait until reaching the takeoff altitude
-- 3) Go forward to the defined distance
-- 4) Go back to the initial position
-- 5) Change to Land mode


local takeoff_alt = 3         -- Takeoff height
local copter_guided_mode_num = 4
local copter_land_mode_num = 9
local stage = 0
local count = 0               -- Number of times the drone has gone forward
local max_count = 2           -- Maximum number of times the drone should go forward
local ping_pong_distance = 10 -- Distance up to which the drone should go forward (m)
local vel = 1    

Next, there's the main function of the code, the update() function, which will be called once the code is started (since it wasn't indicated a waiting time at the last line of the code) and its return indicates that this same fucntion will be called again 100ms after finishing its executuion.

function update()
    -- [Code]
    return update(), 100
end

return update()

In the presented if, the code is waiting for the drone to be armed to change to the next stage. The is_armed() function from the arming library is used to check if the drone is armed or not. The send_text() function from the gcs library is used to send a message ("Arming") with severity 6 (information) to the GCS (Ground Control Station). Thus, while the drone is not armed, it remains in stage 0 of the code.

if not arming:is_armed() then
    stage = 0
    gcs:send_text(6, "Arming")

If the drone is armed, it moves to the else section, which presents a behavior for each stage. In stage 0, the drone flight mode is changed to GUIDED MODE. To do this, it's used the function set_mode() of vehicle library, which receives the desired flight mode number(copter_guided_mode_num, defined in the local variables). When it verifies the drone has switched to the desired flight mode, it moves to the next stage.

else
    if stage == 0 then
        if vehicle:set_mode(copter_guided_mode_num) then 
            stage = stage + 1
        end

in stage 1, it's used the start_takeoff() function from the vehicle library for the drone to take off to a height defined by the variable takeoff_alt.

elseif stage == 1 then
    gcs:send_text(6, "Taking off")
    if vehicle:start_takeoff(takeoff_alt) then
        stage = stage + 1
    end

In stage 2, it's used the ahrs:get_home() function to get the drone's takeoff location, and the ahrs:get_position() function to get the drone's current position. In line 4, it checks that the obtained values are not null. Then, the home:get_distance_NED() function stores in vec_from_home a 3D vector, starting from curr_loc and ending at home. In line 7, the code sends to the GCS the value contained in the z-coordinate of vec_from_home, i.e., the current altitude of the drone (multiplied by -1 since the vector points towards home, which is at a lower altitude than the current position).

When the difference between takeoff_alt and vec_from_home:z() is less than 1, indicating that the drone has reached the takeoff altitude, it moves to the next stage (math.abs() is used to get the absolute value, and we performed an addition instead of subtraction because the z component is negative).

elseif stage == 2 then
    local home = ahrs:get_home()
    local curr_loc = ahrs:get_position()
    if home and curr_loc then 
        local vec_from_home = home:get_distance_NED(curr_loc)
        gcs:send_text(6, "Altitude above home: " .. tostring(math.floor(-vec_from_home:z())))
        if math.abs(takeoff_alt + vec_from_home:z()) < 1 then
            stage = stage + 1
        end
    end

In stage3, first, the drone checks if it has already performed all the requested loops specified in max_count. If so, it switches to stage5. If not, it executes the commands of stage3.

First, it creates a 3D vector target_vel to store the desired velocity. Then, it sets the value of each of the components of this created vector (0 for y and z, and vel for x).

Then, it uses the vehicle:set_target_velocity_NED(target_vel) function to set the drone's velocity as that of target_vel. If it encounters any issues, it sends a warning message.

elseif (stage == 3) then -- Stage 3: Moving Forward
    -- If the maximum number of times is exceeded, move to stage 5
    if (count >= max_count) then
        stage = stage + 2
    end

    -- Calculate velocity vector
    local target_vel = Vector3f()
    target_vel:x(vel)
    target_vel:y(0)
    target_vel:z(0)

    -- Send velocity request
    if not (vehicle:set_target_velocity_NED(target_vel)) then
        gcs:send_text(6, "Failed to execute velocity command")
    end

After that, it uses the same strategy as shown before to calculate the distance vector from the takeoff location to verify the distance x traveled. When the difference between the distance x that the drone traveled and the ping_pong_distance is less than 1, it increments the count and moves to the next stage.

    -- checking if reached stop point
    local home = ahrs:get_home()
    local curr_loc = ahrs:get_position()
    if home and curr_loc then 
        local vec_from_home = home:get_distance_NED(curr_loc)
        gcs:send_text(6, "Distance from home: " .. tostring(math.floor(vec_from_home:x())))
        if(math.abs(ping_pong_distance - vec_from_home:x()) < 1) then
            count = count + 1
            stage = stage + 1
        end
    end
end

In Stage4, the velocity in x is multiplied by -1 for the drone to fly in the opposite direction, returning to the takeoff position. When the difference between the drone's position x and the takeoff position's x is less than 1, it goes back to Stage3 to start the forward movement again (or not, if the count has reached max_count).

elseif (stage == 4) then -- Stage 4: Moving Back
    -- calculate velocity vector
    local target_vel = Vector3f()
    target_vel:x(-vel)
    target_vel:y(0)
    target_vel:z(0)

    -- send velocity request
    if not (vehicle:set_target_velocity_NED(target_vel)) then
        gcs:send_text(6, "Failed to execute velocity command")
    end

    -- checking if reached stop point
    local home = ahrs:get_home()
    local curr_loc = ahrs:get_position()
    if home and curr_loc then 
        local vec_from_home = home:get_distance_NED(curr_loc)
        gcs:send_text(6, "Distance from home: " .. tostring(math.floor(vec_from_home:x())))
        if(math.abs(vec_from_home:x()) < 1) then
            stage = stage - 1
        end
    end
end

In stage5, the drone simply changes to the Land flight mode and lands, indicating the completion of the code through a message.

elseif (stage == 5) then -- Stage 5: Change to LAND mode
    vehicle:set_mode(copter_rtl_mode_num)
    stage = stage + 1
    gcs:send_text(6, "Finished pingpong, switching to LAND")
end