Skip to content

Instantly share code, notes, and snippets.

@seksitha
Created February 6, 2024 14:44
Show Gist options
  • Save seksitha/c75a68aa2023d34811e338dc633d1501 to your computer and use it in GitHub Desktop.
Save seksitha/c75a68aa2023d34811e338dc633d1501 to your computer and use it in GitHub Desktop.
-- 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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment