AP_Scripting: examples: add fake camera tracking

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