From 3874e80393b6da80195e9f162d42d43921761a77 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 8 Oct 2024 08:20:12 +1100 Subject: [PATCH] AP_Scripting: examples: add fake camera tracking --- .../examples/fake_camera_tracking.lua | 163 ++++++++++++++++++ 1 file changed, 163 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..cbabc9db983752 --- /dev/null +++ b/libraries/AP_Scripting/examples/fake_camera_tracking.lua @@ -0,0 +1,163 @@ +-- 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 = 3 } +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) +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() + + 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") + end +end + +-- Check a command long message for a relevant message interval command +local function handle_command_long(msg) + if msg.msgid ~= COMMAND_LONG_ID then + return + end + if msg.command ~= MAV_CMD_SET_MESSAGE_INTERVAL then + return + end + if msg.param1 ~= CAMERA_TRACKING_IMAGE_STATUS_ID then + return + end + camera_tracking_interval_ms = msg.param2 +end + +-- Check for message interval commands +local function check_message_interval_commands() + while true do + local msg, chan = mavlink_msgs.receive_chan() + if not msg then + break + end + if not camera_tracking_channel then + camera_tracking_channel = chan + end + if camera_tracking_channel == chan then + local parsed_msg = mavlink_msgs.decode(msg, msg_map) + if parsed_msg then + handle_command_long(parsed_msg) + end + end + end +end + +local function send_tracking_status() + -- Check if we even have a channel to send messages on + if not camera_tracking_channel then + return + end + local now = millis():toint() + -- Handle wraparound + if now < camera_tracking_last_ms then + camera_tracking_last_ms = now + 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_status = {} + if tracking_active then + tracking_status.tracking_status = CAMERA_TRACKING_STATUS_FLAGS.ACTIVE + else + tracking_status.tracking_status = CAMERA_TRACKING_STATUS_FLAGS.IDLE + end + tracking_status.tracking_mode = last_tracking_type + tracking_status.target_data = CAMERA_TRACKING_TARGET_DATA.IN_STATUS + tracking_status.point_x = NAN + tracking_status.point_y = NAN + tracking_status.radius = NAN + tracking_status.rec_top_x = NAN + tracking_status.rec_top_y = NAN + tracking_status.rec_bottom_x = NAN + tracking_status.rec_bottom_y = NAN + if last_tracking_type == CAMERA_TRACKING_MODE.POINT then + tracking_status.point_x = tracking_p1x + tracking_status.point_y = tracking_p1y + end + if last_tracking_type == CAMERA_TRACKING_MODE.RECTANGLE then + tracking_status.rec_top_x = tracking_p1x + tracking_status.rec_top_y = tracking_p1y + tracking_status.rec_bottom_x = tracking_p1x + (last_tracking_p2x - last_tracking_p1x) + tracking_status.rec_bottom_y = tracking_p1y + (last_tracking_p2y - last_tracking_p1y) + end + + -- Send tracking status message + mavlink:send_chan(camera_tracking_channel, mavlink_msgs.encode("CAMERA_TRACKING_IMAGE_STATUS", tracking_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