Waypoint and Location difference Lua

ArduRover Rover 4.0
1

I try to write a lua script to solve the automatic and manual dump with a boat. The manual mode works perfectly, the problem with my automatice mode. I dont know exatly how to get my boat target location and the boat location. I try work from Here

I hope you can help me
Thanks a lots :slight_smile:

My code:

local uritbezar = true
local work_flag = false
local mode = 0 -- 0 - ürítés 1 - automata 
local wp_radius = 2
local is_target_reached_flag = false
function work_function()
            
    if uritbezar then
                SRV_Channels:set_output_pwm_chan_timeout(4, 1915, 1000) 
                gcs:send_text(6, "urit---")
        		SRV_Channels:set_output_pwm_chan_timeout(5, 927, 1000) 
                gcs:send_text(6, "urit---")
            else
                SRV_Channels:set_output_pwm_chan_timeout(4, 970, 1000) 
                gcs:send_text(6, "---bezar")
        		SRV_Channels:set_output_pwm_chan_timeout(5, 1857, 1000) 
                gcs:send_text(6, "---bezar")
        		work_flag = false
            end
            uritbezar = not uritbezar

        end 

    function ch_checker()
    	local ch7 = rc:get_pwm(7)
    	gcs:send_text(6, "Chanel check")
    	if ch7 == 1514 then
    		mode_code = 0
    		return mode_code, true
    		
    	elseif ch7 == 2003 then 
    		mode_code = 1
    		if is_target_reached_flag then
    			work_flag = true
    		end
    		
    		return mode_code, is_target_reached_flag
    		
    	elseif ch7 == 1024 then
    		mode_code = 0
    		work_flag = true
    		return mode_code, false
    		
    	end
    	
    end

function is_target_reached()
	local error_c = false
	
	gcs:send_text(0, "cel_vizsgalat")
	
	local target = vehicle:get_target_location()
	local curr_loc = ahrs:get_position()
	
	if not target then
		gcs:send_text(0, "Target hiba")
		error_c = true
	end
	
	if not curr_loc then
		gcs:send_text(0, "Location hiba")
		error_c = true
	end
	
	if error_c then
		return
	end
	
	gcs:send_text(6, string.format("Target: %d %d", target:lat(), target:lng()))
	gcs:send_text(6, string.format("Location: %d %d", curr_loc:lat(), curr_loc:lng()))
		
	local target_dist = curr_loc:get_distance(target)
	
	gcs:send_text(6, string.format("Distance: %d", target_dist))

	if (target_dist < wp_radius) then
		gcs:send_text(0, "cel_elerve")
        is_target_reached_flag = true
	else
		is_target_reached_flag = false
    end
	
	return

end

function update()
	mode, state = ch_checker()
	gcs:send_text(6, string.format("ModCode: %d", mode))
	if state and work_flag  then
		work_function()
		return update, 7000
	end
	
	if mode == 1 then
		gcs:send_text(0, "Automata mode")
		is_target_reached()
	elseif mode == 0 then
		gcs:send_text(0, "Manual mode")
	end
	
	return update, 1000 
	
end

gcs:send_text(6, "a_scripttel_vezerelt_urites_mukodik")
return update, 5000
1 Like