mirror of https://github.com/ArduPilot/ardupilot
61 lines
2.1 KiB
Lua
61 lines
2.1 KiB
Lua
|
-- cause a rover to drive in a figure of eight pattern by directly controlling steering and throttle
|
||
|
|
--
|
||
|
|
-- CAUTION: This script only works for Rover
|
||
|
|
-- this script waits for the vehicle to be armed and RC6 input > 1800 and then:
|
||
|
|
-- a) switches to Guided mode
|
||
|
|
-- b) increases throttle to 30% and turns right at 20% for 10 seconds
|
||
|
|
-- c) keep throttle at 30% and turns left at 20% for 10 seconds
|
||
|
|
-- d) switches to Hold mode
|
||
|
|
|
||
|
|
local stage = 0
|
||
|
|
local stage_counter = 0
|
||
|
|
local rover_guided_mode_num = 15
|
||
|
|
local rover_hold_mode_num = 4
|
||
|
|
|
||
|
|
-- the main update function that directly sets throttle and steering out to drive a figure of eight pattern
|
||
|
|
function update()
|
||
|
|
if not arming:is_armed() then -- reset state when disarmed
|
||
|
|
stage = 0
|
||
|
|
stage_counter = 0
|
||
|
|
else
|
||
|
|
pwm6 = rc:get_pwm(6)
|
||
|
|
if pwm6 and pwm6 > 1800 then -- check if RC6 input has moved high
|
||
|
|
if (stage == 0) then -- change to guided mode
|
||
|
|
if (vehicle:set_mode(rover_guided_mode_num)) then -- change to Guided mode
|
||
|
|
stage = stage + 1
|
||
|
|
stage_counter = 0
|
||
|
|
end
|
||
|
|
elseif (stage == 1) then -- Stage1: increase throttle to 30% and turn right 20%
|
||
|
|
if (vehicle:set_steering_and_throttle(0.2, 0.3)) then
|
||
|
|
stage_counter = stage_counter + 1
|
||
|
|
if (stage_counter >= 10) then
|
||
|
|
stage = stage + 1
|
||
|
|
stage_counter = 0
|
||
|
|
end
|
||
|
|
end
|
||
|
|
elseif (stage == 2) then -- Stage2: keep throttle at 30% and turn left 20%
|
||
|
|
if (vehicle:set_steering_and_throttle(-0.2, 0.3)) then
|
||
|
|
stage_counter = stage_counter + 1
|
||
|
|
if (stage_counter >= 10) then
|
||
|
|
stage = stage + 1
|
||
|
|
stage_counter = 0
|
||
|
|
end
|
||
|
|
end
|
||
|
|
elseif (stage == 3) then -- Stage3: change to Hold mode
|
||
|
|
vehicle:set_mode(rover_hold_mode_num)
|
||
|
|
stage = stage + 1
|
||
|
|
gcs:send_text(0, "finished, switching to Hold")
|
||
|
|
end
|
||
|
|
else -- RC6 has been moved low
|
||
|
|
if stage > 3 then
|
||
|
|
stage = 0
|
||
|
|
stage_counter = 0
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
return update, 1000
|
||
|
|
end
|
||
|
|
|
||
|
|
return update()
|