local UPDATE_RATE_HZ = 4 local MIN_DIST = 100 local MAX_ALT = 15 local NAV_VTOL_LAND = 85 local NAV_WAYPOINT = 16 local MODE_AUTO = 10 local scripting_rc_1 = rc:find_channel_for_option(300) local sw_pos_alt = 0 -- set wp location function wp_setloc(wp, loc, ctyp) wp:x(loc:lat()) wp:y(loc:lng()) if ctyp == 85 then -- home wp:z(0) else wp:z(loc:alt()*0.01) end end -- add a waypoint function wp_add(loc,ctype,bezug) local wp = mavlink_mission_item_int_t() local seq = mission:num_commands() wp_setloc(wp,loc,ctype) wp:command(ctype) wp:seq(seq) wp:frame(bezug) mission:set_item(seq, wp) end gcs:send_text(4, "ok") function update() local FLIGHTMODE = vehicle:get_mode() local sw_pos = scripting_rc_1:get_aux_switch_pos() if vehicle:get_likely_flying() == true then if FLIGHTMODE ~= MODE_AUTO and sw_pos == 2 then local vector = ahrs:get_relative_position_NED_home() local distance = vector:length() local home_loc = ahrs:get_home() local actual_loc = ahrs:get_location() actual_loc:change_alt_frame(1) -- local altitude = actual_loc:alt()*0.01 if distance < MIN_DIST then -- gcs:send_text(4, "Delta:".. tostring(distance)) -- "too close to hompoint" gcs:send_text(4, "Distance".. tostring(distance)) gcs:send_text(4, "MISSED APPROACH") elseif altitude > MAX_ALT then gcs:send_text(4, "Alt:".. tostring(altitude)) gcs:send_text(4, "MISSED APPROACH") else mission:clear() wp_add(home_loc,NAV_WAYPOINT,0) -- Home position wp_add(actual_loc,NAV_WAYPOINT,3) -- current position wp_add(home_loc,NAV_VTOL_LAND,3) -- VTOL Land vehicle:set_mode(MODE_AUTO) gcs:send_text(4,"APPROACH") end end end sw_pos_alt = sw_pos return update, 1000/UPDATE_RATE_HZ end return update(), 5000