mirror of https://github.com/ArduPilot/ardupilot
133 lines
3.5 KiB
Lua
133 lines
3.5 KiB
Lua
-- 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
|
|
|
|
---@diagnostic disable: cast-local-type
|
|
|
|
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()
|