AP_Scripting: Add Mavlink example
This commit is contained in:
Stephen Dade
Peter Barker
parent
d4bb4e4526
commit
0b048b4b42
460
libraries/AP_Scripting/examples/MAVLinkHL.lua
Normal file
460
libraries/AP_Scripting/examples/MAVLinkHL.lua
Normal file
@ -0,0 +1,460 @@
|
||||
--[[
|
||||
Lua script to simulate a HL connection over a UART
|
||||
Setup:
|
||||
This script requires 1 serial port:
|
||||
A "Script" serial port to connect directly to the GCS
|
||||
|
||||
Usage:
|
||||
Use the MAVLink High Latency Control ("link hl on|off" in MAVProxy) to control
|
||||
whether to send or not
|
||||
The script will, however, automatically enable MAVLink High Latency Control upon start
|
||||
|
||||
Caveats:
|
||||
-This will send HIGH_LATENCY2 packets in place of HEARTBEAT packets
|
||||
-A single HIGH_LATENCY2 packet will be send every 5 sec
|
||||
-MAVLink 1 will be used, as it's slightly more efficient (50 vs 52 bytes for a HL2 message)
|
||||
-The param SCR_VM_I_COUNT may need to be increased in some circumstances
|
||||
|
||||
Written by Stephen Dade (stephen_dade@hotmail.com)
|
||||
--]]
|
||||
local port = serial:find_serial(0)
|
||||
|
||||
if not port or baud == 0 then
|
||||
gcs:send_text(0, "No Scripting Serial Port")
|
||||
return
|
||||
end
|
||||
|
||||
port:begin(19200)
|
||||
port:set_flow_control(0)
|
||||
|
||||
local time_last_tx = millis():tofloat() * 0.001
|
||||
|
||||
-- enable high latency mode from here, instead of having to enable from GCS
|
||||
gcs:enable_high_latency_connections(true)
|
||||
|
||||
--[[
|
||||
Lua Object for decoding and encoding MAVLink (V1 only) messages
|
||||
--]]
|
||||
local function MAVLinkProcessor()
|
||||
-- public fields
|
||||
local self = {
|
||||
-- define MAVLink message id's
|
||||
COMMAND_LONG = 76,
|
||||
COMMAND_INT = 75,
|
||||
HIGH_LATENCY2 = 235,
|
||||
MISSION_ITEM_INT = 73
|
||||
}
|
||||
|
||||
-- private fields
|
||||
local _mavbuffer = ""
|
||||
local _mavresult = {}
|
||||
local _payload_len = 0
|
||||
local _mavdecodestate = 0 -- 0=looking for marker, 1=getting header,2=getting payload,3=getting crc
|
||||
PROTOCOL_MARKER_V1 = 0xFE
|
||||
HEADER_LEN_V1 = 6
|
||||
local _txseqid = 0
|
||||
|
||||
-- AUTOGEN from MAVLink generator
|
||||
_crc_extra = {}
|
||||
_crc_extra[75] = 0x9e
|
||||
_crc_extra[76] = 0x98
|
||||
_crc_extra[235] = 0xb3
|
||||
_crc_extra[73] = 0x26
|
||||
|
||||
_messages = {}
|
||||
_messages[75] = { -- COMMAND_INT
|
||||
{"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"},
|
||||
{"x", "<i4"}, {"y", "<i4"}, {"z", "<f"}, {"command", "<I2"},
|
||||
{"target_system", "<B"}, {"target_component", "<B"}, {"frame", "<B"},
|
||||
{"current", "<B"}, {"autocontinue", "<B"}
|
||||
}
|
||||
_messages[76] = { -- COMMAND_LONG
|
||||
{"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"},
|
||||
{"param5", "<f"}, {"param6", "<f"}, {"param7", "<f"},
|
||||
{"command", "<I2"}, {"target_system", "<B"}, {"target_component", "<B"},
|
||||
{"confirmation", "<B"}
|
||||
}
|
||||
_messages[235] = { -- HIGH_LATENCY2
|
||||
{"timestamp", "<I4"}, {"latitude", "<i4"}, {"longitude", "<i4"},
|
||||
{"custom_mode", "<I2"}, {"altitude", "<i2"}, {"target_altitude", "<i2"},
|
||||
{"target_distance", "<I2"}, {"wp_num", "<I2"}, {"failure_flags", "<I2"},
|
||||
{"type", "<B"}, {"autopilot", "<B"}, {"heading", "<B"},
|
||||
{"target_heading", "<B"}, {"throttle", "<B"}, {"airspeed", "<B"},
|
||||
{"airspeed_sp", "<B"}, {"groundspeed", "<B"}, {"windspeed", "<B"},
|
||||
{"wind_heading", "<B"}, {"eph", "<B"}, {"epv", "<B"},
|
||||
{"temperature_air", "<b"}, {"climb_rate", "<b"}, {"battery", "<b"},
|
||||
{"custom0", "<B"}, -- should be <b (int8), but we're hacking this into a uint8 instead
|
||||
{"custom1", "<b"}, {"custom2", "<b"}
|
||||
}
|
||||
_messages[73] = { -- MISSION_ITEM_INT
|
||||
{"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"},
|
||||
{"x", "<i4"}, {"y", "<i4"}, {"z", "<f"}, {"seq", "<I2"},
|
||||
{"command", "<I2"}, {"target_system", "<B"}, {"target_component", "<B"},
|
||||
{"frame", "<B"}, {"current", "<B"}, {"autocontinue", "<B"}
|
||||
}
|
||||
|
||||
function self.getSeqID() return _txseqid end
|
||||
|
||||
function self.generateCRC(buffer)
|
||||
-- generate the x25crc for a given buffer. Make sure to include crc_extra!
|
||||
local crc = 0xFFFF
|
||||
for i = 1, #buffer do
|
||||
local tmp = string.byte(buffer, i, i) ~ (crc & 0xFF)
|
||||
tmp = (tmp ~ (tmp << 4)) & 0xFF
|
||||
crc = (crc >> 8) ~ (tmp << 8) ~ (tmp << 3) ~ (tmp >> 4)
|
||||
crc = crc & 0xFFFF
|
||||
end
|
||||
return string.pack("<H", crc)
|
||||
end
|
||||
|
||||
function self.parseMAVLink(byte)
|
||||
-- parse a new byte and see if we've got MAVLink message
|
||||
-- returns true if a packet was decoded, false otherwise
|
||||
_mavbuffer = _mavbuffer .. string.char(byte)
|
||||
|
||||
-- parse buffer to find MAVLink packets
|
||||
if #_mavbuffer == 1 and string.byte(_mavbuffer, 1) == PROTOCOL_MARKER_V1 and
|
||||
_mavdecodestate == 0 then
|
||||
-- we have a packet start
|
||||
_mavdecodestate = 1
|
||||
return
|
||||
end
|
||||
|
||||
-- if we have a full header, try parsing
|
||||
if #_mavbuffer == HEADER_LEN_V1 and _mavdecodestate == 1 then
|
||||
local read_marker = 1
|
||||
_, read_marker = string.unpack("<B", _mavbuffer, read_marker)
|
||||
_payload_len, read_marker = string.unpack("<B", _mavbuffer,
|
||||
read_marker) -- payload is always the second byte
|
||||
-- fetch seq/sysid/compid
|
||||
_mavresult.seq, _mavresult.sysid, _mavresult.compid, read_marker =
|
||||
string.unpack("<BBB", _mavbuffer, read_marker)
|
||||
-- fetch the message id
|
||||
_mavresult.msgid, _ =
|
||||
string.unpack("<B", _mavbuffer, read_marker)
|
||||
|
||||
_mavdecodestate = 2
|
||||
return
|
||||
end
|
||||
|
||||
-- get payload
|
||||
if _mavdecodestate == 2 and #_mavbuffer ==
|
||||
(_payload_len + HEADER_LEN_V1) then
|
||||
_mavdecodestate = 3
|
||||
_mavresult.payload = string.sub(_mavbuffer, HEADER_LEN_V1 + 1)
|
||||
return
|
||||
end
|
||||
|
||||
-- get crc, then process if CRC ok
|
||||
if _mavdecodestate == 3 and #_mavbuffer ==
|
||||
(_payload_len + HEADER_LEN_V1 + 2) then
|
||||
_mavdecodestate = 0
|
||||
_mavresult.crc = string.sub(_mavbuffer, -2, -1)
|
||||
|
||||
local message_map = _messages[_mavresult.msgid]
|
||||
if not message_map then
|
||||
-- we don't know how to decode this message, bail on it
|
||||
_mavbuffer = ""
|
||||
return true
|
||||
end
|
||||
|
||||
-- check CRC, if message defined
|
||||
local crc_extra_msg = _crc_extra[_mavresult.msgid]
|
||||
if crc_extra_msg ~= nil then
|
||||
local calccrc = self.generateCRC(
|
||||
string.sub(_mavbuffer, 2, -3) ..
|
||||
string.char(crc_extra_msg))
|
||||
if _mavresult.crc ~= calccrc then
|
||||
gcs:send_text(3,
|
||||
"Bad CRC: " ..
|
||||
self.bytesToString(_mavbuffer, -2, -1) ..
|
||||
", " .. self.bytesToString(calccrc, 1, 2))
|
||||
_mavbuffer = ""
|
||||
return
|
||||
end
|
||||
end
|
||||
|
||||
-- map all the fields out
|
||||
local offset = 1
|
||||
for _, v in ipairs(message_map) do
|
||||
if v[3] then
|
||||
_mavresult[v[1]] = {}
|
||||
for j = 1, v[3] do
|
||||
_mavresult[v[1]][j], offset = string.unpack(v[2],
|
||||
_mavresult.payload,
|
||||
offset)
|
||||
end
|
||||
else
|
||||
_mavresult[v[1]], offset = string.unpack(v[2],
|
||||
_mavresult.payload,
|
||||
offset)
|
||||
end
|
||||
end
|
||||
-- only process COMMAND_LONG and COMMAND_INT and MISSION_ITEM_INT messages
|
||||
if _mavresult.msgid == self.MISSION_ITEM_INT then
|
||||
-- goto somewhere (guided mode target)
|
||||
if _mavresult.command == 16 then -- MAV_CMD_NAV_WAYPOINT
|
||||
local loc = Location()
|
||||
loc:lat(_mavresult.x)
|
||||
loc:lng(_mavresult.y)
|
||||
loc:alt(_mavresult.z * 100)
|
||||
if _mavresult.frame == 10 then -- MAV_FRAME_GLOBAL_TERRAIN_ALT
|
||||
loc:terrain_alt(true)
|
||||
elseif _mavresult.frame == 3 then -- MAV_FRAME_GLOBAL_RELATIVE_ALT
|
||||
loc:relative_alt(true)
|
||||
end
|
||||
vehicle:set_target_location(loc)
|
||||
end
|
||||
elseif _mavresult.msgid == self.COMMAND_LONG or _mavresult.msgid ==
|
||||
self.COMMAND_INT then
|
||||
if _mavresult.command == 400 then -- MAV_CMD_COMPONENT_ARM_DISARM
|
||||
if _mavresult.param1 == 1 then
|
||||
arming:arm()
|
||||
elseif _mavresult.param1 == 0 then
|
||||
arming:disarm()
|
||||
end
|
||||
elseif _mavresult.command == 176 then -- MAV_CMD_DO_SET_MODE
|
||||
vehicle:set_mode(_mavresult.param2)
|
||||
elseif _mavresult.command == 20 then -- MAV_CMD_NAV_RETURN_TO_LAUNCH (Mode RTL) may vary depending on frame
|
||||
if FWVersion:type() == 2 then -- copter
|
||||
vehicle:set_mode(6)
|
||||
elseif FWVersion:type() == 3 then -- plane
|
||||
vehicle:set_mode(11)
|
||||
elseif FWVersion:type() == 1 then -- rover
|
||||
vehicle:set_mode(11)
|
||||
end
|
||||
elseif _mavresult.command == 21 then -- MAV_CMD_NAV_LAND (Mode LAND) may vary depending on frame
|
||||
if FWVersion:type() == 2 then -- copter
|
||||
vehicle:set_mode(9)
|
||||
elseif FWVersion:type() == 12 then -- blimp
|
||||
vehicle:set_mode(0)
|
||||
end
|
||||
elseif _mavresult.command == 22 then -- MAV_CMD_NAV_TAKEOFF
|
||||
vehicle:start_takeoff(_mavresult.param7)
|
||||
elseif _mavresult.command == 84 then -- MAV_CMD_NAV_VTOL_TAKEOFF
|
||||
vehicle:start_takeoff(_mavresult.param7)
|
||||
elseif _mavresult.command == 85 then -- MAV_CMD_NAV_VTOL_LAND (Mode QLAND)
|
||||
vehicle:set_mode(20)
|
||||
elseif _mavresult.command == 300 then -- MAV_CMD_MISSION_START --mode auto and then start mission
|
||||
if FWVersion:type() == 2 then -- copter
|
||||
vehicle:set_mode(3)
|
||||
elseif FWVersion:type() == 3 then -- plane
|
||||
vehicle:set_mode(10)
|
||||
elseif FWVersion:type() == 1 then -- rover
|
||||
vehicle:set_mode(10)
|
||||
elseif FWVersion:type() == 7 then -- sub
|
||||
vehicle:set_mode(3)
|
||||
end
|
||||
elseif _mavresult.command == 2600 then -- MAV_CMD_CONTROL_HIGH_LATENCY
|
||||
if _mavresult.param1 == 1 then
|
||||
gcs:enable_high_latency_connections(true)
|
||||
else
|
||||
gcs:enable_high_latency_connections(false)
|
||||
end
|
||||
end
|
||||
end
|
||||
_mavbuffer = ""
|
||||
return true
|
||||
end
|
||||
|
||||
-- packet too big ... start again
|
||||
if #_mavbuffer > 263 then _mavbuffer = "" end
|
||||
return false
|
||||
end
|
||||
|
||||
function self.bytesToString(buf, start, stop)
|
||||
local ret = ""
|
||||
for idx = start, stop do
|
||||
ret = ret .. string.format("0x%x ", buf:byte(idx), 1, -1) .. " "
|
||||
end
|
||||
return ret
|
||||
end
|
||||
|
||||
function self.createMAVLink(message, msgid)
|
||||
-- generate a mavlink message (V1 only)
|
||||
|
||||
-- create the payload
|
||||
local message_map = _messages[msgid]
|
||||
if not message_map then
|
||||
-- we don't know how to encode this message, bail on it
|
||||
gcs:send_text(3, "Unknown MAVLink message " .. msgid)
|
||||
return nil
|
||||
end
|
||||
|
||||
local packString = "<"
|
||||
local packedTable = {}
|
||||
local packedIndex = 1
|
||||
for i, v in ipairs(message_map) do
|
||||
if v[3] then
|
||||
packString = (packString ..
|
||||
string.rep(string.sub(v[2], 2), v[3]))
|
||||
for j = 1, v[3] do
|
||||
packedTable[packedIndex] = message[message_map[i][1]][j]
|
||||
packedIndex = packedIndex + 1
|
||||
end
|
||||
else
|
||||
packString = (packString .. string.sub(v[2], 2))
|
||||
packedTable[packedIndex] = message[message_map[i][1]]
|
||||
packedIndex = packedIndex + 1
|
||||
end
|
||||
end
|
||||
|
||||
local payload = string.pack(packString, table.unpack(packedTable))
|
||||
|
||||
-- create the header. Assume componentid of 1
|
||||
local header = string.pack('<BBBBBB', PROTOCOL_MARKER_V1, #payload,
|
||||
_txseqid, param:get('SYSID_THISMAV'), 1,
|
||||
msgid)
|
||||
|
||||
-- generate the CRC
|
||||
local crc_extra_msg = _crc_extra[msgid]
|
||||
local crc = self.generateCRC(string.sub(header, 2) .. payload ..
|
||||
string.char(crc_extra_msg))
|
||||
|
||||
-- iterate sequence id
|
||||
_txseqid = (_txseqid + 1) % 255
|
||||
|
||||
return header .. payload .. crc
|
||||
end
|
||||
|
||||
-- return the instance
|
||||
return self
|
||||
end
|
||||
|
||||
-- Transmitted HIGH_LATENCY2 packet
|
||||
local hl2 = {}
|
||||
hl2.timestamp = 0
|
||||
hl2.latitude = 0
|
||||
hl2.longitude = 0
|
||||
hl2.custom_mode = 0
|
||||
hl2.altitude = 0
|
||||
hl2.target_altitude = 0
|
||||
hl2.target_distance = 0
|
||||
hl2.wp_num = 0
|
||||
hl2.failure_flags = 0
|
||||
hl2.type = gcs:frame_type()
|
||||
hl2.autopilot = 3 -- MAV_AUTOPILOT_ARDUPILOTMEGA
|
||||
hl2.heading = 0
|
||||
hl2.target_heading = 0
|
||||
hl2.throttle = 0
|
||||
hl2.airspeed = 0
|
||||
hl2.airspeed_sp = 0
|
||||
hl2.groundspeed = 0
|
||||
hl2.windspeed = 0
|
||||
hl2.wind_heading = 0
|
||||
hl2.eph = 0
|
||||
hl2.epv = 0
|
||||
hl2.temperature_air = 0
|
||||
hl2.climb_rate = 0
|
||||
hl2.battery = 0
|
||||
hl2.custom0 = 1 -- MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
hl2.custom1 = 0
|
||||
hl2.custom2 = 0
|
||||
|
||||
function wrap_360(angle)
|
||||
local res = angle % 360
|
||||
if res < 0 then res = res + 360 end
|
||||
return res
|
||||
end
|
||||
|
||||
-- Define the MAVLink processor
|
||||
local mavlink = MAVLinkProcessor()
|
||||
|
||||
function HLSatcom()
|
||||
-- read in any bytes from GCS and and send to MAVLink processor
|
||||
-- only read in 1 packet at a time to avoid time overruns
|
||||
while port:available() > 0 do
|
||||
local byte = port:read()
|
||||
if mavlink.parseMAVLink(byte) then break end
|
||||
end
|
||||
|
||||
-- send HL2 packet every 5 sec
|
||||
if gcs:get_high_latency_status() and (millis():tofloat() * 0.001) -
|
||||
time_last_tx > 5 then
|
||||
|
||||
-- update HL2 packet
|
||||
hl2.timestamp = millis():tofloat()
|
||||
local position = ahrs:get_location()
|
||||
local wind = ahrs:wind_estimate()
|
||||
|
||||
if position then
|
||||
hl2.latitude = tonumber(position:lat())
|
||||
hl2.longitude = tonumber(position:lng())
|
||||
hl2.altitude = math.floor(tonumber(position:alt()) * 0.01)
|
||||
end
|
||||
if wind then
|
||||
wind_xy = Vector2f()
|
||||
wind_xy:x(wind:x())
|
||||
wind_xy:y(wind:y())
|
||||
hl2.windspeed = math.abs(math.floor(wind_xy:length() * 5))
|
||||
hl2.wind_heading = math.floor(wrap_360(wind_xy:angle()) / 2)
|
||||
end
|
||||
hl2.custom_mode = vehicle:get_mode()
|
||||
|
||||
if vehicle:get_wp_distance_m() ~= nil then
|
||||
hl2.target_distance = math.floor(vehicle:get_wp_distance_m() / 10)
|
||||
end
|
||||
if mission:get_current_nav_index() ~= nil then
|
||||
hl2.wp_num = mission:get_current_nav_index()
|
||||
end
|
||||
if vehicle:get_wp_bearing_deg() ~= nil then
|
||||
hl2.target_heading = math.floor(wrap_360(
|
||||
vehicle:get_wp_bearing_deg()) /
|
||||
2)
|
||||
end
|
||||
|
||||
-- failure flags
|
||||
hl2.failure_flags = 0
|
||||
if not ahrs:healthy() then
|
||||
hl2.failure_flags = hl2.failure_flags + 4096 -- HL_FAILURE_FLAG_ESTIMATOR
|
||||
end
|
||||
if battery:num_instances() > 0 and not battery:healthy(0) then
|
||||
hl2.failure_flags = hl2.failure_flags + 128 -- HL_FAILURE_FLAG_BATTERY
|
||||
end
|
||||
if gps:num_sensors() > 0 and gps:status(0) <= gps.NO_FIX then
|
||||
hl2.failure_flags = hl2.failure_flags + 1 -- HL_FAILURE_FLAG_GPS
|
||||
end
|
||||
if (FWVersion:type() == 2 or FWVersion:type() == 3) and terrain:status() ==
|
||||
terrain.TerrainStatusUnhealthy then
|
||||
-- only for copter and plane
|
||||
hl2.failure_flags = hl2.failure_flags + 64 -- HL_FAILURE_FLAG_TERRAIN
|
||||
end
|
||||
if not rc:has_valid_input() then
|
||||
hl2.failure_flags = hl2.failure_flags + 256 -- HL_FAILURE_FLAG_RC_RECEIVER
|
||||
end
|
||||
|
||||
hl2.heading = math.floor(wrap_360(math.deg(ahrs:get_yaw())) / 2)
|
||||
hl2.throttle = math.floor(gcs:get_hud_throttle())
|
||||
if ahrs:airspeed_estimate() ~= nil then
|
||||
hl2.airspeed = math.abs(math.floor(ahrs:airspeed_estimate() * 5))
|
||||
end
|
||||
-- hl2.airspeed_sp = 0
|
||||
hl2.groundspeed = math.abs(math.floor(
|
||||
ahrs:groundspeed_vector():length() * 5))
|
||||
|
||||
hl2.temperature_air = math.floor(baro:get_external_temperature())
|
||||
hl2.battery = battery:capacity_remaining_pct(0)
|
||||
|
||||
-- just sending armed state here for simplicity. Flight mode is in the custom_mode field
|
||||
if arming:is_armed() then
|
||||
hl2.custom0 = 129 -- MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
else
|
||||
hl2.custom0 = 1 -- MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
end
|
||||
|
||||
local newpkt = mavlink.createMAVLink(hl2, mavlink.HIGH_LATENCY2)
|
||||
gcs:send_text(3,
|
||||
"Sent HL2 packet, size: " .. tostring(#newpkt) .. ", seq " ..
|
||||
mavlink.getSeqID())
|
||||
|
||||
for idx = 1, #newpkt do port:write(newpkt:byte(idx)) end
|
||||
|
||||
time_last_tx = millis():tofloat() * 0.001
|
||||
|
||||
end
|
||||
|
||||
return HLSatcom, 100
|
||||
end
|
||||
|
||||
return HLSatcom, 100
|
||||
|
||||
Loading…
Reference in New Issue
Block a user