-- Home Position Sender -- Sends the flight controller's home position to the VNS edge API when QGC -- sends a custom COMMAND_LONG action. -- -- QGC action: -- command: 31000 (MAV_CMD_USER_1) -- params: all zero -- -- Outbound command to the Pi: -- MAV_CMD_DO_SET_HOME (179) -- param1: 0.0 = use specified position -- param5: latitude in degrees -- param6: longitude in degrees -- param7: altitude AMSL in meters -- Add common module locations for SITL/hardware compatibility. package.path = package.path .. ";./scripts/modules/?.lua" package.path = package.path .. ";./scripts/modules/?/init.lua" package.path = package.path .. ";./scripts/modules/MAVLink/?.lua" package.path = package.path .. ";./scripts/mavlink/modules/?.lua" package.path = package.path .. ";./mavlink/modules/?.lua" package.path = package.path .. ";./scripts/MAVLink/?.lua" package.path = package.path .. ";./MAVLink/?.lua" package.path = package.path .. ";./?.lua" local ok_mavlink_msgs, mavlink_msgs_or_err = pcall(require, "MAVLink/mavlink_msgs") local ok_mavlink_utils, mavlink_utils_or_err = pcall(require, "MAVLink/mavlink_utils") if not ok_mavlink_msgs or not ok_mavlink_utils then gcs:send_text(3, "Home Sender error: MAVLink modules missing") local function module_error_update() return module_error_update, 1000 end return module_error_update() end local mavlink_msgs = mavlink_msgs_or_err local mavlink_utils = mavlink_utils_or_err ------------------------------------------------------------- -- CONFIGURATION ------------------------------------------------------------- local VNS_SYSID = 218 local VNS_COMPID = 190 local MAV_CMD_DO_SET_HOME = 179 local MAV_CMD_SEND_HOME_TO_VNS = 31000 local MAV_CMD_REQUEST_MESSAGE = 512 local HOME_POSITION_MSG_ID = 242 ------------------------------------------------------------- -- MAVLink setup ------------------------------------------------------------- local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG") local COMMAND_ACK_ID = mavlink_msgs.get_msgid("COMMAND_ACK") local msg_map = {} msg_map[COMMAND_LONG_ID] = "COMMAND_LONG" msg_map[COMMAND_ACK_ID] = "COMMAND_ACK" mavlink:init(10, 2) mavlink:register_rx_msgid(COMMAND_LONG_ID) mavlink:register_rx_msgid(COMMAND_ACK_ID) mavlink:block_command(MAV_CMD_SEND_HOME_TO_VNS) local available_channels = mavlink_utils.get_available_channels() ------------------------------------------------------------- -- State tracking ------------------------------------------------------------- local init_done = false ------------------------------------------------------------- -- Helpers ------------------------------------------------------------- local function send_command_ack(chan, command, result, target_system, target_component) local ack = {} ack.command = command ack.result = result ack.progress = 0 ack.result_param2 = 0 ack.target_system = target_system ack.target_component = target_component mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack)) end local function get_home_position() local home = nil if vehicle ~= nil and vehicle.home ~= nil then local ok, result = pcall(function() return vehicle:home() end) if ok then home = result end end if home == nil and ahrs ~= nil and ahrs.get_home ~= nil then local ok, result = pcall(function() return ahrs:get_home() end) if ok then home = result end end if home == nil then return nil end local lat = home:lat() local lon = home:lng() local alt = home:alt() if lat == nil or lon == nil or alt == nil then return nil end if lat == 0 and lon == 0 then return nil end return lat / 10000000.0, lon / 10000000.0, alt / 100.0 end local function send_home_position() local lat_deg, lon_deg, alt_m = get_home_position() if lat_deg == nil then gcs:send_text(4, "Home Sender: FC home position is not set") return false end local cmd = {} cmd.target_system = VNS_SYSID cmd.target_component = VNS_COMPID cmd.command = MAV_CMD_DO_SET_HOME cmd.confirmation = 0 cmd.param1 = 0.0 cmd.param2 = 0.0 cmd.param3 = 0.0 cmd.param4 = 0.0 cmd.param5 = lat_deg cmd.param6 = lon_deg cmd.param7 = 0 -- Use terrain data for altitude. might cause a feedback loop otherwise. local msgid, payload = mavlink_msgs.encode("COMMAND_LONG", cmd) mavlink_utils.send_to_all_channels(available_channels, msgid, payload) return true end local function handle_command_ack(parsed) if parsed.command ~= MAV_CMD_DO_SET_HOME then return end if parsed.result == 0 then return elseif parsed.result == 2 then gcs:send_text(4, "Home Sender: Denied by edge API") elseif parsed.result == 4 then gcs:send_text(4, "Home Sender: Edge API failed to set home") else gcs:send_text(4, string.format("Home Sender: Edge API command failed (result=%d)", parsed.result)) end end local function handle_command_long(parsed, chan) if parsed.command == MAV_CMD_REQUEST_MESSAGE and math.floor((parsed.param1 or 0) + 0.5) == HOME_POSITION_MSG_ID and math.floor((parsed.param2 or 0) + 0.5) == MAV_CMD_SEND_HOME_TO_VNS then -- ArduPilot handles and ACKs REQUEST_MESSAGE, so do not send a second -- ACK here. We only use the marked request as a QGC-friendly trigger. send_home_position() return elseif parsed.command == MAV_CMD_SEND_HOME_TO_VNS then local result = send_home_position() and 0 or 4 send_command_ack(chan, MAV_CMD_SEND_HOME_TO_VNS, result, parsed.sysid, parsed.compid) end end local function check_mavlink() local msg, chan = mavlink:receive_chan() if msg == nil then return end local header_ok, header = pcall(mavlink_msgs.decode_header, msg) if not header_ok or header == nil or msg_map[header.msgid] == nil then return end local parsed_ok, parsed = pcall(mavlink_msgs.decode, msg, msg_map) if not parsed_ok or parsed == nil then return end if parsed.msgid == COMMAND_LONG_ID then handle_command_long(parsed, chan) elseif parsed.msgid == COMMAND_ACK_ID then handle_command_ack(parsed) end end ------------------------------------------------------------- -- Main update loop ------------------------------------------------------------- local function update() if not init_done then init_done = true gcs:send_text(6, string.format("Home Sender ready (REQUEST_MESSAGE marker %d, %d MAV channels)", MAV_CMD_SEND_HOME_TO_VNS, #available_channels)) end check_mavlink() return update, 100 end return update()