AP_Scripting: add mount-test example script

This commit is contained in:
Randy Mackay 2022-06-24 17:20:06 +09:00
parent 47e10eaefa
commit b8560345f7
1 changed files with 130 additions and 0 deletions

View File

@ -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()