From 44b604ae2c120e7c0e7fc619fc5a3178c50b8192 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 15 Oct 2024 09:39:09 +1100 Subject: [PATCH] AP_Scripting: examples: add fake camera tracking --- .../examples/fake_camera_tracking.lua | 169 ++++++++++++++++++ 1 file changed, 169 insertions(+) create mode 100644 libraries/AP_Scripting/examples/fake_camera_tracking.lua diff --git a/libraries/AP_Scripting/examples/fake_camera_tracking.lua b/libraries/AP_Scripting/examples/fake_camera_tracking.lua new file mode 100644 index 00000000000000..8cf84d0bd54b66 --- /dev/null +++ b/libraries/AP_Scripting/examples/fake_camera_tracking.lua @@ -0,0 +1,169 @@ +-- constant definitions +local MAV_SEVERITY = { EMERGENCY = 0, ALERT = 1, CRITICAL = 2, ERROR = 3, WARNING = 4, NOTICE = 5, INFO = 6, DEBUG = 7 } +local CAMERA_TRACKING_STATUS_FLAGS = { IDLE = 0, ACTIVE = 1, ERROR = 2 } +local CAMERA_TRACKING_MODE = { NONE = 0, POINT = 1, RECTANGLE = 2 } +local CAMERA_TRACKING_TARGET_DATA = { NONE = 0, EMBEDDED = 1, RENDERED = 2, IN_STATUS = 4 } +local UPDATE_INTERVAL_MS = 100 -- update at 10hz +local CAMERA_INSTANCE = 0 -- always use the first camera +local NAN = 0 / 0 + +-- Tracking state variables +local last_tracking_type = 0 -- last tracking type 0:off 1:point 2:rectangle +local last_tracking_p1x = 0 -- last tracking point 1 x (for detecting changes from GCS) +local last_tracking_p1y = 0 -- last tracking point 1 y (for detecting changes from GCS) +local last_tracking_p2x = 0 -- last tracking point 2 x (for detecting changes from GCS) +local last_tracking_p2y = 0 -- last tracking point 2 y (for detecting changes from GCS) +local tracking_p1x = 0 -- tracking point 1 x (for reporting to GCS) +local tracking_p1y = 0 -- tracking point 1 y (for reporting to GCS) +local tracking_active = false -- tracking is active + +-- MAVLink handling +local mavlink_msgs = require("MAVLink/mavlink_msgs") +local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG") +local CAMERA_TRACKING_IMAGE_STATUS_ID = mavlink_msgs.get_msgid("CAMERA_TRACKING_IMAGE_STATUS") +local msg_map = {} +msg_map[COMMAND_LONG_ID] = "COMMAND_LONG" +msg_map[CAMERA_TRACKING_IMAGE_STATUS_ID] = "CAMERA_TRACKING_IMAGE_STATUS" +mavlink:init(1, 10) +mavlink:register_rx_msgid(COMMAND_LONG_ID) +local camera_tracking_interval_ms = 0 -- interval in milliseconds to send tracking status +local camera_tracking_last_ms = 0 -- last time tracking status was sent +local camera_tracking_channel = nil -- channel to send tracking status (we only support one channel at a time) +local MAV_CMD_SET_MESSAGE_INTERVAL = 511 + +local function update_camera() + -- get latest camera state from AP driver + local cam_state = camera:get_state(CAMERA_INSTANCE) + if not cam_state then + return + end + + -- check for change in tracking state + local tracking_type_changed = cam_state:tracking_type() ~= last_tracking_type + local tracking_type_p1_changed = (cam_state:tracking_p1():x() ~= last_tracking_p1x) or (cam_state:tracking_p1():y() ~= last_tracking_p1y) + local tracking_type_p2_changed = (cam_state:tracking_p2():x() ~= last_tracking_p2x) or (cam_state:tracking_p2():y() ~= last_tracking_p2y) + last_tracking_type = cam_state:tracking_type() + last_tracking_p1x = cam_state:tracking_p1():x() + last_tracking_p1y = cam_state:tracking_p1():y() + last_tracking_p2x = cam_state:tracking_p2():x() + last_tracking_p2y = cam_state:tracking_p2():y() + + -- print("tracking_type: " .. cam_state:tracking_type()) + + if tracking_type_p1_changed then + tracking_p1x = last_tracking_p1x + tracking_p1y = last_tracking_p1y + end + + if (last_tracking_type == CAMERA_TRACKING_MODE.NONE) and tracking_type_changed then + -- turn off tracking + tracking_active = false + gcs:send_text(MAV_SEVERITY.INFO, "Fake Tracking: OFF") + end + + if (last_tracking_type == CAMERA_TRACKING_MODE.POINT) and (tracking_type_changed or tracking_type_p1_changed) then + -- turn tracking point on + tracking_active = true + gcs:send_text(MAV_SEVERITY.INFO, "Fake Tracking: point ON") + end + + if (last_tracking_type == CAMERA_TRACKING_MODE.RECTANGLE) and (tracking_type_changed or tracking_type_p1_changed or tracking_type_p2_changed) then + -- turn tracking rectangle on + tracking_active = true + gcs:send_text(MAV_SEVERITY.INFO, "Fake Tracking: rectangle ON") + print("tracking_p1x: " .. tracking_p1x) + print("tracking_p1y: " .. tracking_p1y) + print("tracking_p2x: " .. last_tracking_p2x) + print("tracking_p2y: " .. last_tracking_p2y) + end +end + +-- Check a command long message for a relevant message interval command +local function handle_command_long(chan, msg) + if msg.msgid ~= COMMAND_LONG_ID then + return + end + if msg.command ~= MAV_CMD_SET_MESSAGE_INTERVAL then + return + end + print("got message interval command " .. msg.param1 .. " " .. msg.param2 / 1000) + if msg.param1 ~= CAMERA_TRACKING_IMAGE_STATUS_ID then + return + end + camera_tracking_interval_ms = msg.param2 / 1000 + camera_tracking_channel = msg.target_system +end + +-- Check for message interval commands +local function check_message_interval_commands() + while true do + local msg, chan = mavlink:receive_chan() + if not msg then + break + end + if not camera_tracking_channel or camera_tracking_channel == chan then + local parsed_msg = mavlink_msgs.decode(msg, msg_map) + if parsed_msg then + handle_command_long(chan, parsed_msg) + end + end + end +end + +local function send_tracking_status() + -- Check if we even have a channel to send messages on + local now = millis():toint() + if not camera_tracking_channel then + return + end + -- Check if this message is disabled + if camera_tracking_interval_ms <= 0 then + return + end + -- Check if it is time to send the message + if camera_tracking_last_ms + camera_tracking_interval_ms > now then + return + end + camera_tracking_last_ms = now + + -- Construct tracking status message + local tracking_image_status = {} + if tracking_active then + tracking_image_status.tracking_status = CAMERA_TRACKING_STATUS_FLAGS.ACTIVE + else + tracking_image_status.tracking_status = CAMERA_TRACKING_STATUS_FLAGS.IDLE + end + tracking_image_status.tracking_mode = last_tracking_type + tracking_image_status.target_data = CAMERA_TRACKING_TARGET_DATA.IN_STATUS + tracking_image_status.point_x = NAN + tracking_image_status.point_y = NAN + tracking_image_status.radius = NAN + tracking_image_status.rec_top_x = NAN + tracking_image_status.rec_top_y = NAN + tracking_image_status.rec_bottom_x = NAN + tracking_image_status.rec_bottom_y = NAN + if last_tracking_type == CAMERA_TRACKING_MODE.POINT then + tracking_image_status.point_x = tracking_p1x + tracking_image_status.point_y = tracking_p1y + end + if last_tracking_type == CAMERA_TRACKING_MODE.RECTANGLE then + tracking_image_status.rec_top_x = tracking_p1x + tracking_image_status.rec_top_y = tracking_p1y + tracking_image_status.rec_bottom_x = tracking_p1x + (last_tracking_p2x - last_tracking_p1x) + tracking_image_status.rec_bottom_y = tracking_p1y + (last_tracking_p2y - last_tracking_p1y) + end + tracking_image_status.camera_device_id = 1 + + -- Send tracking status message + mavlink:send_chan(camera_tracking_channel, mavlink_msgs.encode("CAMERA_TRACKING_IMAGE_STATUS", tracking_image_status)) +end + +local function update() + check_message_interval_commands() + update_camera() + send_tracking_status() + + return update, UPDATE_INTERVAL_MS +end + +return update, UPDATE_INTERVAL_MS