fly_mission_do_circular.lua

-- Todo : breakpoint continue and 
--      : go to specific point
--      : stop and spray first then go circle
--      : edit guided mode to change alt base on point throttle
--      : guided mode takeout surface track and use radar alt direct
--      : radar alt measured from the top of the tree



-- Edit these variables
local rad_xy_m = 3.0   -- circle radius in xy plane in m
local target_speed_xy_mps = 5.0     -- maximum target speed in m/s
local ramp_up_time_s = 10.0     -- time to reach target_speed_xy_mps in second
local sampling_time_s = 0.05
local angle = 0    -- sampling time of script
local mission_command = 2

-- Fixed variables
local omega_radps = target_speed_xy_mps/rad_xy_m  -- 10m/5m-per-sec
local theta = 0.0
local time = 0.0
local timer = 0
local do_cirle = false
local curNEU = Vector3f(0,0,0)

--local is_set_target_pos = false;
gcs:send_text(6,"Script started")
gcs:send_text(6,"Trajectory period: " .. tostring(2 * math.rad(180) / omega_radps))
--local RC3 = rc:get_channel(3)
local stage = 0
local fly_mission = false
function circle(x, y)
    local cur_freq = 0
    -- increase target speed lineary with time until ramp_up_time_s is reached
    if time <= ramp_up_time_s then 
        -- cur_freq = omega_radps*(time/ramp_up_time_s)^2
    else 
        cur_freq = omega_radps --moving radian 2m/s 
    end

    -- calculate circle reference position and velocity
    theta = theta + cur_freq + sampling_time_s


    local th_s = math.sin(theta)
    local th_c = math.cos(theta) 
    -- local angToRadian = angle * math.pi/180
    local pos = Vector3f(0,0,0)
    -- negative angle is (-angle) will run clockwise but not correct radius length
    -- where positive is correct length
    -- 
    pos:x(x+rad_xy_m*math.cos(math.rad(angle))) 
    pos:y(y+(-(rad_xy_m*math.sin(math.rad(angle)))))
    pos:z(0) --in meter not cm

    local vel = Vector3f()
  
    --vel:x(rad_xy_m*math.cos(math.rad(angle)))
    --vel:y(-(rad_xy_m*math.sin(math.rad(angle))))
    vel:x(0)
    vel:y(0)
    vel:z(0)
    angle = angle + 2
    return pos, vel
end

function circle_run(center_loc)
    local target_pos = Vector3f(0.0,0.0,0.0)
    local target_vel = Vector3f()
    target_pos, target_vel = circle(center_loc:x(),center_loc:y())
    target_pos:z(center_loc:z()) 
    if not vehicle:set_target_posvel_NED(target_pos, target_vel) then
        gcs:send_text(6, "Failed to send target posvel at " .. tostring(time) .. " seconds")
    end
end

function update()
   
    -- rc 7 1700+
    if rc:get_pwm(7) > 1560 then
        stage = 3
    elseif rc:get_pwm(7) < 1060 and stage > 0 and not arming:is_armed() then
        stage = 0
        mission_command = 2
        fly_mission = false
        gcs:send_text(6, "takeoff start ...")
    elseif rc:get_pwm(7) < 1060 and stage > 0 and arming:is_armed() then
        -- fly goto on QGC
        stage = 1
        mission_command = 2
        fly_mission = false
        gcs:send_text(6, "can go to start ...")
        
    elseif rc:get_pwm(7) > 1450 and rc:get_pwm(7) < 1560 then
        stage = 2
        gcs:send_text(6, "stage 2")
    end


    -- arm and takeoff stage0
    if(arming:is_armed() and stage == 0)then
        vehicle:set_mode(4)
        -- gcs:send_text(0, "mode loiter success can arm")
        if vehicle:start_takeoff(10) then
            stage = 1
        end
    end


    if arming:is_armed() and vehicle:get_mode() == 4 and stage == 2 and mission_command < mission:num_commands() then
        local cur_alt = ahrs:get_location()
        local mission_item = mission:get_item(mission_command)
        local itm = Location()
        itm:lat(mission_item:x())
        itm:lng(mission_item:y())
        itm:alt(cur_alt:alt())
        
        local itemNEU = Vector3f(0,0,0)
        itemNEU= itm.get_vector_from_origin_NEU(itm)
        --convert to meter
        itemNEU:x(itemNEU:x()*0.01)
        itemNEU:y(itemNEU:y()*0.01)
        itemNEU:z(-itemNEU:z()*0.01)

        if  fly_mission == false then
            if vehicle:set_target_posvel_NED(itemNEU,Vector3f(0,0,0)) then
                fly_mission = true
                gcs:send_text(6, "we are going to mission: "..mission_command .." alt: "..itemNEU:z())
            end
        elseif fly_mission == true then
            local cur = ahrs:get_location() -- location
            local target = Location()
            target:lat(mission_item:x())
            target:lng(mission_item:y())
            local dist = cur:get_distance(target)
            -- gcs:send_text(6, "dist to mission: "..dist)
            if dist < 0.50 and not do_cirle then --Todo this is not a good way to check
                do_cirle = true
                local cur_center = ahrs:get_location()
                curNEU = cur_center.get_vector_from_origin_NEU(cur_center)
                -- convert to cm
                curNEU:x(curNEU:x()*0.01)
                curNEU:y(curNEU:y()*0.01)
                curNEU:z(-curNEU:z()*0.01)
            end
            if do_cirle then 
                if angle > 420 then 
                    fly_mission = false 
                    mission_command = mission_command +1
                    angle=0
                    do_cirle = false
                    gcs:send_text(6, "go to next mision ...")
                end
                if angle <= 420 then 
                    circle_run(curNEU) 
                    gcs:send_text(6, "spray circle...: "..angle)
                    -- timer = timer + 1
                end
            end

        end
    end
    return update,  100
end

return update()