defmodule XMAVLink.Frame do
@moduledoc """
Represent and work with MAVLink v1/2 message frames
"""
require Logger
import XMAVLink.Utils, only: [x25_crc: 1, x25_crc: 2]
defstruct [
# Which raw attributes are populated?
version: nil,
payload_length: nil,
# MAVLink 2 only
incompatible_flags: 0,
# MAVLink 2 only
compatible_flags: 0,
sequence_number: nil,
source_system: nil,
source_component: nil,
# Default to broadcast assumed elsewhere
target_system: 0,
target_component: 0,
target: nil,
message_id: nil,
crc_extra: nil,
payload: nil,
checksum: nil,
# MAVLink 2 signing only (not implemented)
signature: nil,
# Original binary frame
mavlink_1_raw: nil,
mavlink_2_raw: nil,
message: nil
]
@type message :: XMAVLink.Message.t()
@type version :: 1 | 2
@type t :: %XMAVLink.Frame{
version: version,
payload_length: 1..255,
incompatible_flags: non_neg_integer,
compatible_flags: non_neg_integer,
sequence_number: 0..255,
source_system: 1..255,
source_component: 1..255,
target_system: 1..255,
target_component: 1..255,
target: :broadcast | :system | :system_component | :component,
message_id: XMAVLink.Types.message_id(),
crc_extra: XMAVLink.Types.crc_extra(),
payload: binary,
checksum: pos_integer,
mavlink_1_raw: binary,
mavlink_2_raw: binary,
message: message
}
@spec binary_to_frame_and_tail(binary) ::
{XMAVLink.Frame.t(), binary} | {nil, binary} | :not_a_frame
# MAVLink version 1
def binary_to_frame_and_tail(
raw_and_rest =
<<0xFE, payload_length::unsigned-integer-size(8),
sequence_number::unsigned-integer-size(8), source_system::unsigned-integer-size(8),
source_component::unsigned-integer-size(8), message_id::unsigned-integer-size(8),
payload::binary-size(payload_length), checksum::little-unsigned-integer-size(16),
rest::binary>>
) do
{
struct(XMAVLink.Frame,
version: 1,
payload_length: payload_length,
sequence_number: sequence_number,
source_system: source_system,
source_component: source_component,
message_id: message_id,
payload: payload,
checksum: checksum,
mavlink_1_raw:
binary_part(
raw_and_rest,
0,
byte_size(raw_and_rest) - byte_size(rest)
)
),
rest
}
end
# MAVLink version 2
def binary_to_frame_and_tail(
raw_and_rest =
<<0xFD, payload_length::unsigned-integer-size(8),
incompatible_flags::unsigned-integer-size(8),
compatible_flags::unsigned-integer-size(8), sequence_number::unsigned-integer-size(8),
source_system::unsigned-integer-size(8), source_component::unsigned-integer-size(8),
message_id::little-unsigned-integer-size(24), payload::binary-size(payload_length),
checksum::little-unsigned-integer-size(16), rest::binary>>
) do
case incompatible_flags do
0 ->
# Vanilla MAVLink 2, we can deal with this
{
struct(XMAVLink.Frame,
version: 2,
payload_length: payload_length,
incompatible_flags: 0,
compatible_flags: compatible_flags,
sequence_number: sequence_number,
source_system: source_system,
source_component: source_component,
message_id: message_id,
payload: payload,
checksum: checksum,
mavlink_2_raw:
binary_part(
raw_and_rest,
0,
byte_size(raw_and_rest) - byte_size(rest)
)
),
rest
}
_ ->
# We don't support any incompatible flags at present
# e.g. signing, so drop the frame
{nil, rest}
end
end
def binary_to_frame_and_tail(unfinished_mavlink_1_frame = <<0xFE, _::binary>>),
do: {nil, unfinished_mavlink_1_frame}
def binary_to_frame_and_tail(unfinished_mavlink_2_frame = <<0xFD, _::binary>>),
do: {nil, unfinished_mavlink_2_frame}
def binary_to_frame_and_tail(<<_, rest::binary>>), do: binary_to_frame_and_tail(rest)
def binary_to_frame_and_tail(<<>>), do: :not_a_frame
@spec validate_and_unpack(XMAVLink.Frame.t(), module) ::
{:ok, XMAVLink.Frame.t()} | :failed_to_unpack | :checksum_invalid | :unknown_message
def validate_and_unpack(
frame = %XMAVLink.Frame{message_id: message_id, version: version, payload: payload},
dialect
) do
case apply(dialect, :msg_attributes, [message_id]) do
{:ok, crc_extra, expected_length, target} ->
if frame.checksum ==
:binary.bin_to_list(
%{1 => frame.mavlink_1_raw, 2 => frame.mavlink_2_raw}[frame.version],
{1, frame.payload_length + %{1 => 5, 2 => 9}[frame.version]}
)
|> x25_crc()
|> x25_crc([crc_extra]) do
# Only used to undo MAVLink 2 payload truncation
payload_truncated_length = 8 * (expected_length - frame.payload_length)
# Too many ways for unpack to fail with dodgy messages...
try do
case apply(dialect, :unpack, [
message_id,
version,
payload <>
if(payload_truncated_length > 0 and version > 1,
do: <<0::size(payload_truncated_length)>>,
else: <<>>
)
]) do
{:ok, message} ->
case target do
:broadcast ->
{:ok,
struct(frame,
message: message,
target_system: 0,
target_component: 0,
target: target,
crc_extra: crc_extra
)}
:system ->
{:ok,
struct(frame,
message: message,
target_system: message.target_system,
target_component: 0,
target: target,
crc_extra: crc_extra
)}
:system_component ->
{:ok,
struct(frame,
message: message,
target_system: message.target_system,
target_component: message.target_component,
target: target,
crc_extra: crc_extra
)}
:component ->
{:ok,
struct(frame,
message: message,
target_system: 0,
target_component: message.target_component,
target: target,
crc_extra: crc_extra
)}
end
_ ->
:failed_to_unpack
end
rescue
_ ->
:ok =
Logger.debug(
"validate_and_unpack: Failed to unpack #{inspect(frame)}, couldn't match payload"
)
:failed_to_unpack
end
else
:ok = Logger.debug("validate_and_unpack: Checksum invalid #{inspect(frame)}")
:checksum_invalid
end
_ ->
:ok = Logger.debug("validate_and_unpack: Unknown message #{inspect(frame)}")
:unknown_message
end
end
# Pack message frame
def pack_frame(frame = %XMAVLink.Frame{version: 1}) do
payload_length = byte_size(frame.payload)
mavlink_1_frame =
<<payload_length::unsigned-integer-size(8), frame.sequence_number::unsigned-integer-size(8),
frame.source_system::unsigned-integer-size(8),
frame.source_component::unsigned-integer-size(8),
frame.message_id::little-unsigned-integer-size(8), frame.payload::binary>>
frame
|> struct(
mavlink_1_raw: <<0xFE>> <> mavlink_1_frame <> checksum(mavlink_1_frame, frame.crc_extra)
)
end
def pack_frame(frame = %XMAVLink.Frame{version: 2}) do
{truncated_length, truncated_payload} = truncate_payload(frame.payload)
mavlink_2_frame =
<<
truncated_length::unsigned-integer-size(8),
# Incompatible flags
0::unsigned-integer-size(8),
# Compatible flags
0::unsigned-integer-size(8),
frame.sequence_number::unsigned-integer-size(8),
frame.source_system::unsigned-integer-size(8),
frame.source_component::unsigned-integer-size(8),
frame.message_id::little-unsigned-integer-size(24),
truncated_payload::binary
>>
struct(frame,
mavlink_2_raw: <<0xFD>> <> mavlink_2_frame <> checksum(mavlink_2_frame, frame.crc_extra)
)
end
# MAVLink 2 truncate trailing 0s in payload
defp truncate_payload(payload) do
truncated_payload = String.replace_trailing(payload, <<0>>, "")
if byte_size(truncated_payload) == 0 do
# First byte of payload never truncated
{1, <<0>>}
else
{byte_size(truncated_payload), truncated_payload}
end
end
# Calculate checksum
defp checksum(frame, crc_extra) do
cs = x25_crc(frame <> <<crc_extra::unsigned-integer-size(8)>>)
<<cs::little-unsigned-integer-size(16)>>
end
end
