mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: add mount-test example script
This commit is contained in:
parent
47e10eaefa
commit
b8560345f7
|
@ -0,0 +1,130 @@
|
|||
-- moves 3-axis gimbal (aka "mount") using earth-frame and body-frame rates and angles
|
||||
--
|
||||
-- stage 0: move gimbal to neutral position
|
||||
-- stage 1: yaw CW at 10deg/s in body-frame
|
||||
-- stage 2: yaw CCW at 10 deg/s in body-frame
|
||||
-- stage 3: pitch at 10 deg/s in body-frame
|
||||
-- stage 4: pitch at -10 deg/s in body-frame
|
||||
-- stage 5: roll at 10 deg/s in body-frame
|
||||
-- stage 6: roll at -10 deg/s in body-frame
|
||||
-- stage 7: point North
|
||||
-- stage 8: point South and center
|
||||
-- stage 9: point North and Down
|
||||
-- stage 10: move angle to neutral position
|
||||
|
||||
local stage = 0
|
||||
local stage_time_ms = 5000
|
||||
local stage_start_time_ms = 0
|
||||
local last_stage = 10
|
||||
|
||||
-- the main update function that performs a simplified version of RTL
|
||||
function update()
|
||||
|
||||
-- get current system time
|
||||
local now_ms = millis()
|
||||
|
||||
-- start
|
||||
if stage_start_time_ms == 0 then
|
||||
stage_start_time_ms = now_ms
|
||||
end
|
||||
|
||||
-- check if time to move to next stage
|
||||
local update_user = false
|
||||
if (now_ms - stage_start_time_ms > stage_time_ms) and (stage < last_stage) then
|
||||
stage = stage + 1
|
||||
stage_start_time_ms = now_ms
|
||||
update_user = true
|
||||
end
|
||||
|
||||
if stage == 0 or stage >= last_stage then
|
||||
-- move angle to neutral position
|
||||
mount:set_angle_target(0, 0, 0, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " move to neutral position")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 1 then
|
||||
-- yaw CW at 10deg/s in body-frame
|
||||
mount:set_rate_target(0, 0, 0, 10, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " yaw at 10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 2 then
|
||||
-- yaw CCW at 10 deg/s in body-frame
|
||||
mount:set_rate_target(0, 0, 0, -10, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " yaw at -10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 3 then
|
||||
-- pitch at 10 deg/s in body-frame
|
||||
mount:set_rate_target(0, 0, 10, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " pitch at 10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 4 then
|
||||
-- pitch at -10 deg/s in body-frame
|
||||
mount:set_rate_target(0, 0, -10, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " pitch at -10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 5 then
|
||||
-- roll at 10 deg/s in body-frame
|
||||
mount:set_rate_target(0, 10, 0, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " roll at 10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 6 then
|
||||
-- roll at -10 deg/s in body-frame
|
||||
mount:set_rate_target(0, -10, 0, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " roll at -10deg/s")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 7 then
|
||||
-- point North
|
||||
mount:set_angle_target(0, 0, 0, 0, true)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " point North")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 8 then
|
||||
-- point South and center
|
||||
mount:set_angle_target(0, 0, 0, 180, true)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " point South")
|
||||
end
|
||||
end
|
||||
|
||||
if stage == 9 then
|
||||
-- point North and Down
|
||||
mount:set_angle_target(0, 0, -90, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " point Down")
|
||||
end
|
||||
end
|
||||
|
||||
if stage > last_stage then
|
||||
-- move angle to neutral position
|
||||
mount:set_angle_target(0, 0, 0, 0, false)
|
||||
if update_user then
|
||||
gcs:send_text(6, "stage:" .. tostring(stage) .. " done!")
|
||||
end
|
||||
end
|
||||
|
||||
return update, 100
|
||||
end
|
||||
|
||||
return update()
|
Loading…
Reference in New Issue