///|
/// Per-node link health and telemetry status tracked by `HeartbeatMonitor`.
pub(all) struct NodeLinkStatus {
sysid : Byte
compid : Byte
mut mav_type : Byte
mut autopilot : Byte
mut base_mode : Byte
mut custom_mode : UInt32
mut system_status : Byte
mut armed : Bool
mut last_seen_ms : UInt64
mut heartbeat_count : Int32
mut received_frames : Int32
mut dropped_frames : Int32
mut last_seq : Int32
} derive(Eq, Debug)
///|
pub extend NodeLinkStatus with Eq::{not_equal, equal}
///|
pub extend NodeLinkStatus with @debug.Debug::{to_repr}
///|
/// Multi-node MAVLink v2 heartbeat and link quality monitor.
pub(all) struct HeartbeatMonitor {
timeout_ms : UInt64
nodes : Array[NodeLinkStatus]
} derive(Eq, Debug)
///|
pub extend HeartbeatMonitor with Eq::{not_equal, equal}
///|
pub extend HeartbeatMonitor with @debug.Debug::{to_repr}
///|
/// Create a new `HeartbeatMonitor` with a configurable link timeout in milliseconds (default 3000 ms).
pub fn HeartbeatMonitor::new(timeout_ms~ : UInt64 = 3000UL) -> HeartbeatMonitor {
{ timeout_ms, nodes: [], }
}
///|
fn find_or_create_node(
monitor : HeartbeatMonitor,
sysid : Byte,
compid : Byte,
now_ms : UInt64,
) -> Int32 {
for i = 0; i < monitor.nodes.length(); i = i + 1 {
if monitor.nodes[i].sysid == sysid && monitor.nodes[i].compid == compid {
return i
}
}
monitor.nodes.push({
sysid,
compid,
mav_type: b'\x00',
autopilot: b'\x00',
base_mode: b'\x00',
custom_mode: 0U,
system_status: b'\x00',
armed: false,
last_seen_ms: now_ms,
heartbeat_count: 0,
received_frames: 0,
dropped_frames: 0,
last_seq: -1,
})
monitor.nodes.length() - 1
}
///|
/// Record any incoming `MavFrame` to update sequence-gap packet loss statistics
/// and automatically extract `HEARTBEAT` (#0) state when present.
pub fn HeartbeatMonitor::on_frame(
self : HeartbeatMonitor,
frame : MavFrame,
now_ms : UInt64,
) -> Unit {
let idx = find_or_create_node(
self,
frame.header.sysid,
frame.header.compid,
now_ms,
)
let node = self.nodes[idx]
node.last_seen_ms = now_ms
node.received_frames = node.received_frames + 1
let cur_seq = frame.header.seq.to_int() & 0xFF
if node.last_seq >= 0 {
let expected = (node.last_seq + 1) & 0xFF
if cur_seq != expected {
let gap = (cur_seq - expected + 256) & 0xFF
node.dropped_frames = node.dropped_frames + gap
}
}
node.last_seq = cur_seq
if frame.header.msgid == 0U {
match
Heartbeat::decode_payload(frame.payload, v2=frame.header.version == V2) {
Ok(hb) => {
node.mav_type = hb.mav_type
node.autopilot = hb.autopilot
node.base_mode = hb.base_mode
node.custom_mode = hb.custom_mode
node.system_status = hb.system_status
node.armed = (hb.base_mode.to_int() & 0x80) != 0
node.heartbeat_count = node.heartbeat_count + 1
}
Err(_) => ()
}
}
}
///|
/// Check whether a specific `(sysid, compid)` node is currently alive within `timeout_ms`.
pub fn HeartbeatMonitor::is_node_alive(
self : HeartbeatMonitor,
sysid : Byte,
compid : Byte,
now_ms : UInt64,
) -> Bool {
for node in self.nodes {
if node.sysid == sysid && node.compid == compid {
return now_ms >= node.last_seen_ms &&
now_ms - node.last_seen_ms <= self.timeout_ms &&
node.heartbeat_count > 0
}
}
false
}
///|
/// Look up the current `NodeLinkStatus` for `(sysid, compid)`.
pub fn HeartbeatMonitor::get_node(
self : HeartbeatMonitor,
sysid : Byte,
compid : Byte,
) -> NodeLinkStatus? {
for node in self.nodes {
if node.sysid == sysid && node.compid == compid {
return Some(node)
}
}
None
}
///|
/// Compute the packet loss ratio (`0.0 .. 1.0`) for `(sysid, compid)`.
pub fn HeartbeatMonitor::node_packet_loss_ratio(
self : HeartbeatMonitor,
sysid : Byte,
compid : Byte,
) -> Double {
for node in self.nodes {
if node.sysid == sysid && node.compid == compid {
let total = node.received_frames + node.dropped_frames
if total <= 0 {
return 0.0
}
return node.dropped_frames.to_double() / total.to_double()
}
}
0.0
}
// ============================================================================
// 2. Parameter Protocol Microservice (Client & Server)
// ============================================================================
///|
/// Onboard parameter entry stored in `ParameterServer` or `ParameterClient`.
pub(all) struct ParameterEntry {
param_id : String
mut param_value : Float
param_type : Byte
param_index : UInt32
} derive(Eq, Debug)
///|
pub extend ParameterEntry with Eq::{not_equal, equal}
///|
pub extend ParameterEntry with @debug.Debug::{to_repr}
///|
/// State of the `ParameterClient` synchronization state machine.
pub(all) enum ParamSyncState {
Idle
ListingAll
WritingParam(String, Float, Byte)
Synced
Failed(String)
} derive(Eq, Debug)
///|
pub extend ParamSyncState with Eq::{not_equal, equal}
///|
pub extend ParamSyncState with @debug.Debug::{to_repr}
///|
/// GCS-side MAVLink Parameter Protocol client supporting full list synchronization,
/// missing index gap recovery, and verified `PARAM_SET` transactions.
pub(all) struct ParameterClient {
target_system : Byte
target_component : Byte
timeout_ms : UInt64
max_retries : Int32
mut state : ParamSyncState
mut expected_count : Int32
mut last_action_ms : UInt64
mut retries : Int32
params : Array[ParameterEntry]
} derive(Eq, Debug)
///|
pub extend ParameterClient with Eq::{not_equal, equal}
///|
pub extend ParameterClient with @debug.Debug::{to_repr}
///|
/// Create a new `ParameterClient` targeting `(target_system, target_component)`.
pub fn ParameterClient::new(
target_system : Byte,
target_component : Byte,
timeout_ms~ : UInt64 = 1000UL,
max_retries~ : Int32 = 3,
) -> ParameterClient {
{
target_system,
target_component,
timeout_ms,
max_retries,
state: Idle,
expected_count: -1,
last_action_ms: 0UL,
retries: 0,
params: [],
}
}
///|
/// Initiate a full parameter list download (`PARAM_REQUEST_LIST` #21).
pub fn ParameterClient::start_list_sync(
self : ParameterClient,
now_ms : UInt64,
) -> MavMessage {
self.state = ListingAll
self.expected_count = -1
self.last_action_ms = now_ms
self.retries = 0
self.params.clear()
MsgParamRequestList({
target_system: self.target_system,
target_component: self.target_component,
})
}
///|
/// Initiate a verified parameter write transaction (`PARAM_SET` #23).
pub fn ParameterClient::start_param_set(
self : ParameterClient,
param_id : String,
param_value : Float,
param_type : Byte,
now_ms : UInt64,
) -> MavMessage {
self.state = WritingParam(param_id, param_value, param_type)
self.last_action_ms = now_ms
self.retries = 0
MsgParamSet({
param_value,
target_system: self.target_system,
target_component: self.target_component,
param_id,
param_type,
})
}
///|
/// Process an incoming `PARAM_VALUE` (#22) message from the vehicle.
pub fn ParameterClient::on_param_value(
self : ParameterClient,
pv : ParamValue,
now_ms : UInt64,
) -> Unit {
self.last_action_ms = now_ms
if pv.param_count > 0U {
self.expected_count = pv.param_count.reinterpret_as_int()
}
let mut found = false
for i = 0; i < self.params.length(); i = i + 1 {
if self.params[i].param_id == pv.param_id ||
self.params[i].param_index == pv.param_index {
self.params[i] = {
param_id: pv.param_id,
param_value: pv.param_value,
param_type: pv.param_type,
param_index: pv.param_index,
}
found = true
break
}
}
if !found {
self.params.push({
param_id: pv.param_id,
param_value: pv.param_value,
param_type: pv.param_type,
param_index: pv.param_index,
})
}
match self.state {
ListingAll =>
if self.expected_count > 0 && self.params.length() >= self.expected_count {
self.state = Synced
self.retries = 0
}
WritingParam(target_id, target_val, _) =>
if pv.param_id == target_id && pv.param_value == target_val {
self.state = Synced
self.retries = 0
}
_ => ()
}
}
///|
/// Return the list of missing parameter indices (`0 .. expected_count - 1`) during list synchronization.
pub fn ParameterClient::missing_indices(self : ParameterClient) -> Array[Int32] {
let missing : Array[Int32] = []
if self.expected_count <= 0 {
return missing
}
for idx = 0; idx < self.expected_count; idx = idx + 1 {
let u_idx = idx.reinterpret_as_uint()
let mut seen = false
for p in self.params {
if p.param_index == u_idx {
seen = true
break
}
}
if !seen {
missing.push(idx)
}
}
missing
}
///|
/// Poll for timeout recovery. Emits targeted `PARAM_REQUEST_READ` (#20) for missing indices
/// or retries `PARAM_REQUEST_LIST` / `PARAM_SET` up to `max_retries`.
pub fn ParameterClient::poll_timeout(
self : ParameterClient,
now_ms : UInt64,
) -> MavMessage? {
if now_ms < self.last_action_ms ||
now_ms - self.last_action_ms < self.timeout_ms {
return None
}
match self.state {
ListingAll => {
if self.retries >= self.max_retries {
self.state = Failed("Parameter list sync timed out after max retries")
return None
}
self.retries = self.retries + 1
self.last_action_ms = now_ms
let missing = self.missing_indices()
if missing.length() > 0 {
Some(
MsgParamRequestRead({
param_index: missing[0],
target_system: self.target_system,
target_component: self.target_component,
param_id: "",
}),
)
} else {
Some(
MsgParamRequestList({
target_system: self.target_system,
target_component: self.target_component,
}),
)
}
}
WritingParam(param_id, param_value, param_type) => {
if self.retries >= self.max_retries {
self.state = Failed("PARAM_SET timed out for " + param_id)
return None
}
self.retries = self.retries + 1
self.last_action_ms = now_ms
Some(
MsgParamSet({
param_value,
target_system: self.target_system,
target_component: self.target_component,
param_id,
param_type,
}),
)
}
_ => None
}
}
///|
/// Look up a synchronized parameter by name.
pub fn ParameterClient::get_param(
self : ParameterClient,
param_id : String,
) -> ParameterEntry? {
for p in self.params {
if p.param_id == param_id {
return Some(p)
}
}
None
}
///|
/// Vehicle-side MAVLink Parameter Protocol server supporting `PARAM_REQUEST_LIST`,
/// `PARAM_REQUEST_READ` (by index or ID), and `PARAM_SET`.
pub(all) struct ParameterServer {
sysid : Byte
compid : Byte
params : Array[ParameterEntry]
} derive(Eq, Debug)
///|
pub extend ParameterServer with Eq::{not_equal, equal}
///|
pub extend ParameterServer with @debug.Debug::{to_repr}
///|
/// Create a new `ParameterServer` initialized with a table of parameters.
pub fn ParameterServer::new(
sysid : Byte,
compid : Byte,
initial_params : Array[(String, Float, Byte)],
) -> ParameterServer {
let params : Array[ParameterEntry] = []
for i = 0; i < initial_params.length(); i = i + 1 {
let (id, val, ptype) = initial_params[i]
params.push({
param_id: id,
param_value: val,
param_type: ptype,
param_index: i.reinterpret_as_uint(),
})
}
{ sysid, compid, params, }
}
///|
/// Handle an incoming MAVLink parameter request message and return the response messages.
pub fn ParameterServer::handle_message(
self : ParameterServer,
msg : MavMessage,
) -> Array[MavMessage] {
let total_count = self.params.length().reinterpret_as_uint()
match msg {
MsgParamRequestList(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
let out : Array[MavMessage] = []
for p in self.params {
out.push(
MsgParamValue({
param_value: p.param_value,
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_type: p.param_type,
}),
)
}
out
}
MsgParamRequestRead(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
if req.param_index >= 0 {
let idx = req.param_index
if idx < self.params.length() {
let p = self.params[idx]
return [
MsgParamValue({
param_value: p.param_value,
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_type: p.param_type,
}),
]
}
return []
}
for p in self.params {
if p.param_id == req.param_id {
return [
MsgParamValue({
param_value: p.param_value,
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_type: p.param_type,
}),
]
}
}
[]
}
MsgParamSet(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
for i = 0; i < self.params.length(); i = i + 1 {
if self.params[i].param_id == req.param_id &&
self.params[i].param_type == req.param_type {
self.params[i].param_value = req.param_value
let p = self.params[i]
return [
MsgParamValue({
param_value: p.param_value,
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_type: p.param_type,
}),
]
}
}
[]
}
_ => []
}
}
// ============================================================================
// 3. Mission Protocol Microservice (Upload Client, Download Client & Server)
// ============================================================================
///|
/// State of a MAVLink Mission Upload or Download transaction.
pub(all) enum MissionTxState {
Idle
AwaitingRequest
AwaitingCount
AwaitingItem(UInt32)
Completed
Failed(MavMissionResult)
} derive(Eq, Debug)
///|
pub extend MissionTxState with Eq::{not_equal, equal}
///|
pub extend MissionTxState with @debug.Debug::{to_repr}
///|
/// GCS-side MAVLink Mission Upload state machine (`MISSION_COUNT` -> `MISSION_REQUEST_INT` -> `MISSION_ITEM_INT` -> `MISSION_ACK`).
pub(all) struct MissionUploadClient {
target_system : Byte
target_component : Byte
mission_type : Byte
timeout_ms : UInt64
max_retries : Int32
mut state : MissionTxState
mut last_action_ms : UInt64
mut retries : Int32
mut last_requested_seq : Int32
items : Array[MissionItemInt]
} derive(Eq, Debug)
///|
pub extend MissionUploadClient with Eq::{not_equal, equal}
///|
pub extend MissionUploadClient with @debug.Debug::{to_repr}
///|
/// Create a new `MissionUploadClient`.
pub fn MissionUploadClient::new(
target_system : Byte,
target_component : Byte,
mission_type~ : Byte = b'\x00',
timeout_ms~ : UInt64 = 1500UL,
max_retries~ : Int32 = 5,
) -> MissionUploadClient {
{
target_system,
target_component,
mission_type,
timeout_ms,
max_retries,
state: Idle,
last_action_ms: 0UL,
retries: 0,
last_requested_seq: -1,
items: [],
}
}
///|
/// Start a mission upload transaction by emitting `MISSION_COUNT` (#44).
pub fn MissionUploadClient::start_upload(
self : MissionUploadClient,
waypoints : Array[MissionItemInt],
now_ms : UInt64,
) -> MavMessage {
self.items.clear()
for wp in waypoints {
self.items.push(wp)
}
self.state = AwaitingRequest
self.last_action_ms = now_ms
self.retries = 0
self.last_requested_seq = -1
MsgMissionCount({
count: self.items.length().reinterpret_as_uint(),
target_system: self.target_system,
target_component: self.target_component,
mission_type: self.mission_type,
opaque_id: 0U,
})
}
///|
/// Process an incoming `MISSION_REQUEST_INT` (#51), `MISSION_REQUEST` (#40), or `MISSION_ACK` (#47).
pub fn MissionUploadClient::on_message(
self : MissionUploadClient,
msg : MavMessage,
now_ms : UInt64,
) -> MavMessage? {
if self.state != AwaitingRequest {
return None
}
match msg {
MsgMissionRequestInt(req) => {
self.last_action_ms = now_ms
self.retries = 0
let idx = req.seq.reinterpret_as_int()
if idx >= 0 && idx < self.items.length() {
self.last_requested_seq = idx
Some(MsgMissionItemInt(self.items[idx]))
} else {
self.state = Failed(MavMissionInvalidSequence)
Some(
MsgMissionAck({
target_system: self.target_system,
target_component: self.target_component,
mav_type: MavMissionResult::MavMissionInvalidSequence
.to_uint()
.to_byte(),
mission_type: self.mission_type,
opaque_id: 0U,
}),
)
}
}
MsgMissionRequest(req) => {
self.last_action_ms = now_ms
self.retries = 0
let idx = req.seq.reinterpret_as_int()
if idx >= 0 && idx < self.items.length() {
self.last_requested_seq = idx
Some(MsgMissionItemInt(self.items[idx]))
} else {
self.state = Failed(MavMissionInvalidSequence)
None
}
}
MsgMissionAck(ack) => {
self.last_action_ms = now_ms
match MavMissionResult::from_uint(ack.mav_type.to_uint()) {
Some(MavMissionAccepted) => self.state = Completed
Some(err_code) => self.state = Failed(err_code)
None => self.state = Failed(MavMissionError)
}
None
}
_ => None
}
}
///|
/// Poll for upload timeout retransmission.
pub fn MissionUploadClient::poll_timeout(
self : MissionUploadClient,
now_ms : UInt64,
) -> MavMessage? {
if self.state != AwaitingRequest ||
now_ms < self.last_action_ms ||
now_ms - self.last_action_ms < self.timeout_ms {
return None
}
if self.retries >= self.max_retries {
self.state = Failed(MavMissionError)
return None
}
self.retries = self.retries + 1
self.last_action_ms = now_ms
if self.last_requested_seq >= 0 &&
self.last_requested_seq < self.items.length() {
Some(MsgMissionItemInt(self.items[self.last_requested_seq]))
} else {
Some(
MsgMissionCount({
count: self.items.length().reinterpret_as_uint(),
target_system: self.target_system,
target_component: self.target_component,
mission_type: self.mission_type,
opaque_id: 0U,
}),
)
}
}
///|
/// GCS-side MAVLink Mission Download state machine (`MISSION_REQUEST_LIST` -> `MISSION_COUNT` -> `MISSION_REQUEST_INT` -> `MISSION_ITEM_INT` -> `MISSION_ACK`).
pub(all) struct MissionDownloadClient {
target_system : Byte
target_component : Byte
mission_type : Byte
timeout_ms : UInt64
max_retries : Int32
mut state : MissionTxState
mut expected_count : UInt32
mut last_action_ms : UInt64
mut retries : Int32
downloaded_items : Array[MissionItemInt]
} derive(Eq, Debug)
///|
pub extend MissionDownloadClient with Eq::{not_equal, equal}
///|
pub extend MissionDownloadClient with @debug.Debug::{to_repr}
///|
/// Create a new `MissionDownloadClient`.
pub fn MissionDownloadClient::new(
target_system : Byte,
target_component : Byte,
mission_type~ : Byte = b'\x00',
timeout_ms~ : UInt64 = 1500UL,
max_retries~ : Int32 = 5,
) -> MissionDownloadClient {
{
target_system,
target_component,
mission_type,
timeout_ms,
max_retries,
state: Idle,
expected_count: 0U,
last_action_ms: 0UL,
retries: 0,
downloaded_items: [],
}
}
///|
/// Initiate a mission download transaction (`MISSION_REQUEST_LIST` #43).
pub fn MissionDownloadClient::start_download(
self : MissionDownloadClient,
now_ms : UInt64,
) -> MavMessage {
self.state = AwaitingCount
self.expected_count = 0U
self.last_action_ms = now_ms
self.retries = 0
self.downloaded_items.clear()
MsgMissionRequestList({
target_system: self.target_system,
target_component: self.target_component,
mission_type: self.mission_type,
})
}
///|
/// Process an incoming `MISSION_COUNT` (#44) or `MISSION_ITEM_INT` (#73) during mission download.
pub fn MissionDownloadClient::on_message(
self : MissionDownloadClient,
msg : MavMessage,
now_ms : UInt64,
) -> MavMessage? {
match (self.state, msg) {
(AwaitingCount, MsgMissionCount(mc)) => {
self.last_action_ms = now_ms
self.retries = 0
self.expected_count = mc.count
if mc.count == 0U {
self.state = Completed
Some(
MsgMissionAck({
target_system: self.target_system,
target_component: self.target_component,
mav_type: MavMissionResult::MavMissionAccepted.to_uint().to_byte(),
mission_type: self.mission_type,
opaque_id: 0U,
}),
)
} else {
self.state = AwaitingItem(0U)
Some(
MsgMissionRequestInt({
seq: 0U,
target_system: self.target_system,
target_component: self.target_component,
mission_type: self.mission_type,
}),
)
}
}
(AwaitingItem(expected_seq), MsgMissionItemInt(item)) => {
if item.seq != expected_seq {
return None
}
self.last_action_ms = now_ms
self.retries = 0
self.downloaded_items.push(item)
let next_seq = expected_seq + 1U
if next_seq >= self.expected_count {
self.state = Completed
Some(
MsgMissionAck({
target_system: self.target_system,
target_component: self.target_component,
mav_type: MavMissionResult::MavMissionAccepted.to_uint().to_byte(),
mission_type: self.mission_type,
opaque_id: 0U,
}),
)
} else {
self.state = AwaitingItem(next_seq)
Some(
MsgMissionRequestInt({
seq: next_seq,
target_system: self.target_system,
target_component: self.target_component,
mission_type: self.mission_type,
}),
)
}
}
_ => None
}
}
///|
/// Vehicle-side MAVLink Mission Protocol server supporting upload, download, clear, and current waypoint selection.
pub(all) struct MissionServer {
sysid : Byte
compid : Byte
mut active_seq : UInt32
mut upload_expected : UInt32
mut uploading : Bool
staging_items : Array[MissionItemInt]
stored_items : Array[MissionItemInt]
} derive(Eq, Debug)
///|
pub extend MissionServer with Eq::{not_equal, equal}
///|
pub extend MissionServer with @debug.Debug::{to_repr}
///|
/// Create a new vehicle-side `MissionServer`.
pub fn MissionServer::new(sysid : Byte, compid : Byte) -> MissionServer {
{
sysid,
compid,
active_seq: 0U,
upload_expected: 0U,
uploading: false,
staging_items: [],
stored_items: [],
}
}
///|
/// Handle an incoming MAVLink Mission Protocol message on the vehicle side.
pub fn MissionServer::handle_message(
self : MissionServer,
sender_sysid : Byte,
sender_compid : Byte,
msg : MavMessage,
) -> MavMessage? {
match msg {
MsgMissionCount(mc) => {
self.staging_items.clear()
self.upload_expected = mc.count
if mc.count == 0U {
self.stored_items.clear()
self.uploading = false
Some(
MsgMissionAck({
target_system: sender_sysid,
target_component: sender_compid,
mav_type: MavMissionResult::MavMissionAccepted.to_uint().to_byte(),
mission_type: mc.mission_type,
opaque_id: 1U,
}),
)
} else {
self.uploading = true
Some(
MsgMissionRequestInt({
seq: 0U,
target_system: sender_sysid,
target_component: sender_compid,
mission_type: mc.mission_type,
}),
)
}
}
MsgMissionItemInt(item) => {
if !self.uploading {
return None
}
let expected_seq = self.staging_items.length().reinterpret_as_uint()
if item.seq != expected_seq {
return Some(
MsgMissionAck({
target_system: sender_sysid,
target_component: sender_compid,
mav_type: MavMissionResult::MavMissionInvalidSequence
.to_uint()
.to_byte(),
mission_type: item.mission_type,
opaque_id: 0U,
}),
)
}
self.staging_items.push(item)
let next_seq = expected_seq + 1U
if next_seq >= self.upload_expected {
self.stored_items.clear()
for st in self.staging_items {
self.stored_items.push(st)
}
self.uploading = false
Some(
MsgMissionAck({
target_system: sender_sysid,
target_component: sender_compid,
mav_type: MavMissionResult::MavMissionAccepted.to_uint().to_byte(),
mission_type: item.mission_type,
opaque_id: 1U,
}),
)
} else {
Some(
MsgMissionRequestInt({
seq: next_seq,
target_system: sender_sysid,
target_component: sender_compid,
mission_type: item.mission_type,
}),
)
}
}
MsgMissionRequestList(req) =>
Some(
MsgMissionCount({
count: self.stored_items.length().reinterpret_as_uint(),
target_system: sender_sysid,
target_component: sender_compid,
mission_type: req.mission_type,
opaque_id: 1U,
}),
)
MsgMissionRequestInt(req) => {
let idx = req.seq.reinterpret_as_int()
if idx >= 0 && idx < self.stored_items.length() {
Some(MsgMissionItemInt(self.stored_items[idx]))
} else {
Some(
MsgMissionAck({
target_system: sender_sysid,
target_component: sender_compid,
mav_type: MavMissionResult::MavMissionInvalidSequence
.to_uint()
.to_byte(),
mission_type: req.mission_type,
opaque_id: 0U,
}),
)
}
}
MsgMissionClearAll(req) => {
self.stored_items.clear()
self.staging_items.clear()
self.active_seq = 0U
self.uploading = false
Some(
MsgMissionAck({
target_system: sender_sysid,
target_component: sender_compid,
mav_type: MavMissionResult::MavMissionAccepted.to_uint().to_byte(),
mission_type: req.mission_type,
opaque_id: 0U,
}),
)
}
MsgMissionSetCurrent(req) => {
let idx = req.seq.reinterpret_as_int()
if idx >= 0 && idx < self.stored_items.length() {
self.active_seq = req.seq
Some(
MsgMissionCurrent({
seq: self.active_seq,
total: self.stored_items.length().reinterpret_as_uint(),
mission_state: b'\x03',
mission_mode: b'\x01',
}),
)
} else {
None
}
}
_ => None
}
}
// ============================================================================
// 4. Command Protocol Microservice (COMMAND_LONG / COMMAND_ACK / COMMAND_CANCEL)
// ============================================================================
///|
/// Execution state of a MAVLink Command transaction (`COMMAND_LONG` #76 / `COMMAND_ACK` #77).
pub(all) enum CommandTxState {
Idle
Pending(UInt32, Byte)
InProgress(UInt32, Byte)
Finished(UInt32, MavResult)
TimedOut(UInt32)
} derive(Eq, Debug)
///|
pub extend CommandTxState with Eq::{not_equal, equal}
///|
pub extend CommandTxState with @debug.Debug::{to_repr}
///|
/// GCS-side MAVLink Command Protocol state machine with automatic `confirmation` counter
/// increment on retransmission, `MAV_RESULT_IN_PROGRESS` tracking, and `COMMAND_CANCEL`.
pub(all) struct CommandClient {
target_system : Byte
target_component : Byte
timeout_ms : UInt64
max_retries : Int32
mut state : CommandTxState
mut active_cmd : CommandLong?
mut last_sent_ms : UInt64
mut retries : Int32
} derive(Eq, Debug)
///|
pub extend CommandClient with Eq::{not_equal, equal}
///|
pub extend CommandClient with @debug.Debug::{to_repr}
///|
/// Create a new `CommandClient`.
pub fn CommandClient::new(
target_system : Byte,
target_component : Byte,
timeout_ms~ : UInt64 = 1000UL,
max_retries~ : Int32 = 3,
) -> CommandClient {
{
target_system,
target_component,
timeout_ms,
max_retries,
state: Idle,
active_cmd: None,
last_sent_ms: 0UL,
retries: 0,
}
}
///|
/// Send a `COMMAND_LONG` (#76) with initial `confirmation = 0`.
pub fn CommandClient::send_command_long(
self : CommandClient,
command : UInt32,
now_ms : UInt64,
param1~ : Float = (0.0 : Float),
param2~ : Float = (0.0 : Float),
param3~ : Float = (0.0 : Float),
param4~ : Float = (0.0 : Float),
param5~ : Float = (0.0 : Float),
param6~ : Float = (0.0 : Float),
param7~ : Float = (0.0 : Float),
) -> MavMessage {
let cmd : CommandLong = {
param1,
param2,
param3,
param4,
param5,
param6,
param7,
command,
target_system: self.target_system,
target_component: self.target_component,
confirmation: b'\x00',
}
self.active_cmd = Some(cmd)
self.state = Pending(command, b'\x00')
self.last_sent_ms = now_ms
self.retries = 0
MsgCommandLong(cmd)
}
///|
/// Process an incoming `COMMAND_ACK` (#77) message.
pub fn CommandClient::on_command_ack(
self : CommandClient,
ack : CommandAck,
now_ms : UInt64,
) -> Unit {
let active_id = match self.state {
Pending(cid, _) | InProgress(cid, _) => cid
_ => return ()
}
if ack.command != active_id {
return ()
}
self.last_sent_ms = now_ms
match MavResult::from_uint(ack.result.to_uint()) {
Some(InProgress) => self.state = InProgress(active_id, ack.progress)
Some(final_res) => {
self.state = Finished(active_id, final_res)
self.active_cmd = None
}
None => {
self.state = Finished(active_id, Failed)
self.active_cmd = None
}
}
}
///|
/// Poll for timeout retransmission. Per official MAVLink Command Microservice specification,
/// each retransmission increments the `confirmation` field (`0 -> 1 -> 2...`).
pub fn CommandClient::poll_timeout(
self : CommandClient,
now_ms : UInt64,
) -> MavMessage? {
let cid = match self.state {
Pending(c, _) | InProgress(c, _) => c
_ => return None
}
if now_ms < self.last_sent_ms || now_ms - self.last_sent_ms < self.timeout_ms {
return None
}
if self.retries >= self.max_retries {
self.state = TimedOut(cid)
self.active_cmd = None
return None
}
match self.active_cmd {
Some(cmd) => {
self.retries = self.retries + 1
let next_conf = ((cmd.confirmation.to_int() + 1) & 0xFF).to_byte()
let updated_cmd : CommandLong = { ..cmd, confirmation: next_conf, }
self.active_cmd = Some(updated_cmd)
self.state = Pending(cid, next_conf)
self.last_sent_ms = now_ms
Some(MsgCommandLong(updated_cmd))
}
None => None
}
}
///|
/// Cancel the currently active long-running command by emitting `COMMAND_CANCEL` (#80).
pub fn CommandClient::cancel_active(
self : CommandClient,
now_ms : UInt64,
) -> MavMessage? {
let cid = match self.state {
Pending(c, _) | InProgress(c, _) => c
_ => return None
}
self.last_sent_ms = now_ms
self.state = Finished(cid, Cancelled)
self.active_cmd = None
Some(
MsgCommandCancel({
command: cid,
target_system: self.target_system,
target_component: self.target_component,
}),
)
}
// ============================================================================
// 5. Timesync & Link RTT Estimator (`TIMESYNC` #111)
// ============================================================================
///|
/// Result of a completed two-way `TIMESYNC` (#111) probe.
pub(all) struct TimesyncSample {
rtt_ns : Int64
clock_offset_ns : Int64
} derive(Eq, Debug)
///|
pub extend TimesyncSample with Eq::{not_equal, equal}
///|
pub extend TimesyncSample with @debug.Debug::{to_repr}
///|
/// Two-way MAVLink `TIMESYNC` (#111) clock offset and round-trip latency estimator
/// with exponential moving average (EMA) jitter smoothing.
pub(all) struct TimesyncEstimator {
mut sample_count : Int32
mut min_rtt_ns : Int64
mut ema_rtt_ns : Int64
mut ema_offset_ns : Int64
} derive(Eq, Debug)
///|
pub extend TimesyncEstimator with Eq::{not_equal, equal}
///|
pub extend TimesyncEstimator with @debug.Debug::{to_repr}
///|
/// Create a new `TimesyncEstimator`.
pub fn TimesyncEstimator::new() -> TimesyncEstimator {
{
sample_count: 0,
min_rtt_ns: 0x7FFFFFFFFFFFFFFFL,
ema_rtt_ns: 0L,
ema_offset_ns: 0L,
}
}
///|
/// Create an outbound `TIMESYNC` (#111) probe request (`tc1 = 0`, `ts1 = local_time_ns`).
pub fn TimesyncEstimator::create_probe(
local_time_ns : Int64,
target_system~ : Byte = b'\x01',
target_component~ : Byte = b'\x01',
) -> MavMessage {
MsgTimesync({ tc1: 0L, ts1: local_time_ns, target_system, target_component, })
}
///|
/// Respond to an incoming `TIMESYNC` probe request (`tc1 == 0`) by stamping `tc1 = local_time_ns`.
pub fn TimesyncEstimator::respond_to_probe(
incoming : Timesync,
local_time_ns : Int64,
sender_sysid : Byte,
sender_compid : Byte,
) -> MavMessage? {
if incoming.tc1 != 0L {
return None
}
Some(
MsgTimesync({
tc1: local_time_ns,
ts1: incoming.ts1,
target_system: sender_sysid,
target_component: sender_compid,
}),
)
}
///|
/// Process a returned `TIMESYNC` probe (`tc1 != 0`) at local receive timestamp `rx_time_ns`,
/// updating RTT and clock offset estimates.
pub fn TimesyncEstimator::on_probe_return(
self : TimesyncEstimator,
incoming : Timesync,
rx_time_ns : Int64,
) -> TimesyncSample? {
if incoming.tc1 == 0L || rx_time_ns < incoming.ts1 {
return None
}
let rtt_ns = rx_time_ns - incoming.ts1
let midpoint_ns = incoming.ts1 + rtt_ns / 2L
let clock_offset_ns = incoming.tc1 - midpoint_ns
if rtt_ns < self.min_rtt_ns {
self.min_rtt_ns = rtt_ns
}
if self.sample_count == 0 {
self.ema_rtt_ns = rtt_ns
self.ema_offset_ns = clock_offset_ns
} else {
// Exponential moving average with alpha = 1/4 (integer arithmetic)
self.ema_rtt_ns = (self.ema_rtt_ns * 3L + rtt_ns) / 4L
self.ema_offset_ns = (self.ema_offset_ns * 3L + clock_offset_ns) / 4L
}
self.sample_count = self.sample_count + 1
Some({ rtt_ns, clock_offset_ns, })
}
// ============================================================================
// 6. MAVLink File Transfer Protocol (`FILE_TRANSFER_PROTOCOL` #110) Codec
// ============================================================================
///|
/// Official MAVLink FTP opcodes carried inside the 251-byte `FILE_TRANSFER_PROTOCOL` (#110) payload.
pub(all) enum FtpOpcode {
FtpNone
TerminateSession
ResetSessions
ListDirectory
OpenFileRO
ReadFile
CreateFile
WriteFile
RemoveFile
CreateDirectory
RemoveDirectory
OpenFileWO
TruncateFile
Rename
CalcFileCRC32
BurstReadFile
Ack
Nak
} derive(Eq, Debug)
///|
pub extend FtpOpcode with Eq::{not_equal, equal}
///|
pub extend FtpOpcode with @debug.Debug::{to_repr}
///|
/// Convert `FtpOpcode` to its wire byte code.
pub fn FtpOpcode::to_byte(self : FtpOpcode) -> Byte {
match self {
FtpNone => b'\x00'
TerminateSession => b'\x01'
ResetSessions => b'\x02'
ListDirectory => b'\x03'
OpenFileRO => b'\x04'
ReadFile => b'\x05'
CreateFile => b'\x06'
WriteFile => b'\x07'
RemoveFile => b'\x08'
CreateDirectory => b'\x09'
RemoveDirectory => b'\x0A'
OpenFileWO => b'\x0B'
TruncateFile => b'\x0C'
Rename => b'\x0D'
CalcFileCRC32 => b'\x0E'
BurstReadFile => b'\x0F'
Ack => b'\x80'
Nak => b'\x81'
}
}
///|
/// Decode `FtpOpcode` from its wire byte code.
pub fn FtpOpcode::from_byte(b : Byte) -> FtpOpcode? {
match b.to_int() & 0xFF {
0 => Some(FtpNone)
1 => Some(TerminateSession)
2 => Some(ResetSessions)
3 => Some(ListDirectory)
4 => Some(OpenFileRO)
5 => Some(ReadFile)
6 => Some(CreateFile)
7 => Some(WriteFile)
8 => Some(RemoveFile)
9 => Some(CreateDirectory)
10 => Some(RemoveDirectory)
11 => Some(OpenFileWO)
12 => Some(TruncateFile)
13 => Some(Rename)
14 => Some(CalcFileCRC32)
15 => Some(BurstReadFile)
128 => Some(Ack)
129 => Some(Nak)
_ => None
}
}
///|
/// Structured MAVLink FTP sub-packet (12-byte little-endian header + up to 239 data bytes = 251 bytes max).
pub(all) struct FtpPacket {
seq_number : UInt32
session : Byte
opcode : FtpOpcode
req_opcode : FtpOpcode
burst_complete : Bool
offset : UInt32
data : Bytes
} derive(Eq, Debug)
///|
pub extend FtpPacket with Eq::{not_equal, equal}
///|
pub extend FtpPacket with @debug.Debug::{to_repr}
///|
/// Encode `FtpPacket` into a `FILE_TRANSFER_PROTOCOL` (#110) `MavMessage`.
/// Returns `Err(PayloadTooLarge(_))` if `data.length() > 239`.
pub fn FtpPacket::to_mav_message(
self : FtpPacket,
target_network~ : Byte = b'\x00',
target_system~ : Byte = b'\x01',
target_component~ : Byte = b'\x01',
) -> Result[MavMessage, MavError] {
if self.data.length() > 239 {
return Err(PayloadTooLarge(self.data.length()))
}
let buf = FixedArray::make(251, b'\x00')
buf[0] = (self.seq_number & 0xFFU).to_byte()
buf[1] = ((self.seq_number >> 8) & 0xFFU).to_byte()
buf[2] = self.session
buf[3] = self.opcode.to_byte()
buf[4] = self.data.length().to_byte()
buf[5] = self.req_opcode.to_byte()
buf[6] = if self.burst_complete { b'\x01' } else { b'\x00' }
buf[7] = b'\x00'
buf[8] = (self.offset & 0xFFU).to_byte()
buf[9] = ((self.offset >> 8) & 0xFFU).to_byte()
buf[10] = ((self.offset >> 16) & 0xFFU).to_byte()
buf[11] = ((self.offset >> 24) & 0xFFU).to_byte()
for i = 0; i < self.data.length(); i = i + 1 {
buf[12 + i] = self.data[i]
}
Ok(
MsgFileTransferProtocol({
target_network,
target_system,
target_component,
payload: buf,
}),
)
}
///|
/// Decode an `FtpPacket` from a `FileTransferProtocol` (#110) struct.
pub fn FtpPacket::from_file_transfer_protocol(
ftp : FileTransferProtocol,
) -> Result[FtpPacket, MavError] {
if ftp.payload.length() < 12 {
return Err(PayloadTooShort(12, ftp.payload.length()))
}
let seq_number = ftp.payload[0].to_uint() | (ftp.payload[1].to_uint() << 8)
let session = ftp.payload[2]
let opcode = match FtpOpcode::from_byte(ftp.payload[3]) {
Some(op) => op
None => return Err(SchemaError("Unknown MAVLink FTP opcode"))
}
let data_size = ftp.payload[4].to_int() & 0xFF
if data_size > 239 || 12 + data_size > ftp.payload.length() {
return Err(PayloadTooLarge(data_size))
}
let req_opcode = match FtpOpcode::from_byte(ftp.payload[5]) {
Some(op) => op
None => FtpNone
}
let burst_complete = ftp.payload[6] != b'\x00'
let offset = ftp.payload[8].to_uint() |
(ftp.payload[9].to_uint() << 8) |
(ftp.payload[10].to_uint() << 16) |
(ftp.payload[11].to_uint() << 24)
let data_arr : Array[Byte] = Array(capacity=data_size)
for i = 0; i < data_size; i = i + 1 {
data_arr.push(ftp.payload[12 + i])
}
Ok({
seq_number,
session,
opcode,
req_opcode,
burst_complete,
offset,
data: Bytes::from_array(data_arr),
})
}
// ============================================================================
// 7. Telemetry Stream Rate & Bandwidth Analyzer (`TelemetryRateTracker`)
// ============================================================================
///|
/// Per-message-ID reception counter and byte volume tracked by `TelemetryRateTracker`.
pub(all) struct MessageRateStat {
msgid : UInt32
mut frame_count : Int32
mut payload_bytes : Int32
mut wire_bytes : Int32
} derive(Eq, Debug)
///|
pub extend MessageRateStat with Eq::{not_equal, equal}
///|
pub extend MessageRateStat with @debug.Debug::{to_repr}
///|
/// Telemetry stream analyzer for computing per-message Hz frequency and link bandwidth utilization.
pub(all) struct TelemetryRateTracker {
mut total_frames : Int32
mut total_wire_bytes : Int32
stats : Array[MessageRateStat]
} derive(Eq, Debug)
///|
pub extend TelemetryRateTracker with Eq::{not_equal, equal}
///|
pub extend TelemetryRateTracker with @debug.Debug::{to_repr}
///|
/// Create a new `TelemetryRateTracker`.
pub fn TelemetryRateTracker::new() -> TelemetryRateTracker {
{ total_frames: 0, total_wire_bytes: 0, stats: [], }
}
///|
/// Record a received `MavFrame` and its total wire byte length.
pub fn TelemetryRateTracker::record_frame(
self : TelemetryRateTracker,
frame : MavFrame,
wire_len : Int32,
) -> Unit {
self.total_frames = self.total_frames + 1
self.total_wire_bytes = self.total_wire_bytes + wire_len
let mid = frame.header.msgid
let plen = frame.payload.length()
for i = 0; i < self.stats.length(); i = i + 1 {
if self.stats[i].msgid == mid {
self.stats[i].frame_count = self.stats[i].frame_count + 1
self.stats[i].payload_bytes = self.stats[i].payload_bytes + plen
self.stats[i].wire_bytes = self.stats[i].wire_bytes + wire_len
return ()
}
}
self.stats.push({
msgid: mid,
frame_count: 1,
payload_bytes: plen,
wire_bytes: wire_len,
})
}
///|
/// Compute the average message rate in Hz for `msgid` over a duration of `window_ms` milliseconds.
pub fn TelemetryRateTracker::message_rate_hz(
self : TelemetryRateTracker,
msgid : UInt32,
window_ms : UInt64,
) -> Double {
if window_ms == 0UL {
return 0.0
}
for s in self.stats {
if s.msgid == msgid {
return s.frame_count.to_double() * 1000.0 / window_ms.to_double()
}
}
0.0
}
///|
/// Compute total link bandwidth in Bytes/second over `window_ms` milliseconds.
pub fn TelemetryRateTracker::total_bandwidth_bytes_per_sec(
self : TelemetryRateTracker,
window_ms : UInt64,
) -> Double {
if window_ms == 0UL {
return 0.0
}
self.total_wire_bytes.to_double() * 1000.0 / window_ms.to_double()
}
// ============================================================================
// 8. Extended Parameter Protocol Microservice (`PARAM_EXT_*` #320..#324)
// ============================================================================
///|
/// Extended 128-byte parameter entry for camera, gimbal, and smart payload subsystems.
pub(all) struct ExtParameterEntry {
param_id : String
mut param_value : String
param_type : Byte
param_index : UInt32
} derive(Eq, Debug)
///|
pub extend ExtParameterEntry with Eq::{not_equal, equal}
///|
pub extend ExtParameterEntry with @debug.Debug::{to_repr}
///|
/// Vehicle/payload-side server for the MAVLink Extended Parameter Protocol
/// (`PARAM_EXT_REQUEST_LIST` #321, `PARAM_EXT_REQUEST_READ` #320, `PARAM_EXT_SET` #323 -> `PARAM_EXT_ACK` #324).
pub(all) struct ExtendedParameterServer {
sysid : Byte
compid : Byte
params : Array[ExtParameterEntry]
} derive(Eq, Debug)
///|
pub extend ExtendedParameterServer with Eq::{not_equal, equal}
///|
pub extend ExtendedParameterServer with @debug.Debug::{to_repr}
///|
/// Create a new `ExtendedParameterServer` initialized with `(param_id, param_value, param_type)` entries.
pub fn ExtendedParameterServer::new(
sysid : Byte,
compid : Byte,
initial_params : Array[(String, String, Byte)],
) -> ExtendedParameterServer {
let params : Array[ExtParameterEntry] = []
for i = 0; i < initial_params.length(); i = i + 1 {
let (id, val, ptype) = initial_params[i]
params.push({
param_id: id,
param_value: val,
param_type: ptype,
param_index: i.reinterpret_as_uint(),
})
}
{ sysid, compid, params, }
}
///|
/// Handle an incoming `PARAM_EXT_*` message and return the outbound responses.
pub fn ExtendedParameterServer::handle_message(
self : ExtendedParameterServer,
msg : MavMessage,
) -> Array[MavMessage] {
let total_count = self.params.length().reinterpret_as_uint()
match msg {
MsgParamExtRequestList(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
let out : Array[MavMessage] = []
for p in self.params {
out.push(
MsgParamExtValue({
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_value: p.param_value,
param_type: p.param_type,
}),
)
}
out
}
MsgParamExtRequestRead(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
if req.param_index >= 0 {
let idx = req.param_index
if idx < self.params.length() {
let p = self.params[idx]
return [
MsgParamExtValue({
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_value: p.param_value,
param_type: p.param_type,
}),
]
}
return []
}
for p in self.params {
if p.param_id == req.param_id {
return [
MsgParamExtValue({
param_count: total_count,
param_index: p.param_index,
param_id: p.param_id,
param_value: p.param_value,
param_type: p.param_type,
}),
]
}
}
[]
}
MsgParamExtSet(req) => {
if req.target_system != b'\x00' && req.target_system != self.sysid {
return []
}
for i = 0; i < self.params.length(); i = i + 1 {
if self.params[i].param_id == req.param_id {
if self.params[i].param_type != req.param_type {
return [
MsgParamExtAck({
param_id: self.params[i].param_id,
param_value: self.params[i].param_value,
param_type: self.params[i].param_type,
param_result: ParamAck::ValueUnsupported.to_uint().to_byte(),
}),
]
}
self.params[i].param_value = req.param_value
return [
MsgParamExtAck({
param_id: self.params[i].param_id,
param_value: self.params[i].param_value,
param_type: self.params[i].param_type,
param_result: ParamAck::Accepted.to_uint().to_byte(),
}),
]
}
}
[
MsgParamExtAck({
param_id: req.param_id,
param_value: req.param_value,
param_type: req.param_type,
param_result: ParamAck::Failed.to_uint().to_byte(),
}),
]
}
_ => []
}
}
///|
/// GCS-side client for the MAVLink Extended Parameter Protocol (`PARAM_EXT_*` #320..#324).
pub(all) struct ExtendedParameterClient {
target_system : Byte
target_component : Byte
mut last_ack_result : ParamAck?
params : Array[ExtParameterEntry]
} derive(Eq, Debug)
///|
pub extend ExtendedParameterClient with Eq::{not_equal, equal}
///|
pub extend ExtendedParameterClient with @debug.Debug::{to_repr}
///|
/// Create a new `ExtendedParameterClient`.
pub fn ExtendedParameterClient::new(
target_system : Byte,
target_component : Byte,
) -> ExtendedParameterClient {
{ target_system, target_component, last_ack_result: None, params: [], }
}
///|
/// Request all extended parameters (`PARAM_EXT_REQUEST_LIST` #321).
pub fn ExtendedParameterClient::request_list(
self : ExtendedParameterClient,
) -> MavMessage {
self.params.clear()
MsgParamExtRequestList({
target_system: self.target_system,
target_component: self.target_component,
})
}
///|
/// Send a `PARAM_EXT_SET` (#323) command to update an extended parameter.
pub fn ExtendedParameterClient::set_param(
self : ExtendedParameterClient,
param_id : String,
param_value : String,
param_type : Byte,
) -> MavMessage {
self.last_ack_result = None
MsgParamExtSet({
target_system: self.target_system,
target_component: self.target_component,
param_id,
param_value,
param_type,
})
}
///|
/// Process an incoming `PARAM_EXT_VALUE` (#322) or `PARAM_EXT_ACK` (#324) message.
pub fn ExtendedParameterClient::on_message(
self : ExtendedParameterClient,
msg : MavMessage,
) -> Unit {
match msg {
MsgParamExtValue(pev) => {
for i = 0; i < self.params.length(); i = i + 1 {
if self.params[i].param_id == pev.param_id {
self.params[i] = {
param_id: pev.param_id,
param_value: pev.param_value,
param_type: pev.param_type,
param_index: pev.param_index,
}
return ()
}
}
self.params.push({
param_id: pev.param_id,
param_value: pev.param_value,
param_type: pev.param_type,
param_index: pev.param_index,
})
}
MsgParamExtAck(ack) => {
self.last_ack_result = ParamAck::from_uint(ack.param_result.to_uint())
if self.last_ack_result == Some(Accepted) {
for i = 0; i < self.params.length(); i = i + 1 {
if self.params[i].param_id == ack.param_id {
self.params[i].param_value = ack.param_value
return ()
}
}
}
}
_ => ()
}
}
///|
/// Virtual file entry stored inside `FtpServer`.
pub(all) struct FtpVirtualFile {
path : String
content : Bytes
} derive(Eq, Debug)
///|
pub extend FtpVirtualFile with Eq::{not_equal, equal}
///|
pub extend FtpVirtualFile with @debug.Debug::{to_repr}
///|
/// Vehicle-side MAVLink FTP (`FILE_TRANSFER_PROTOCOL` #110) virtual filesystem server
/// supporting `OpenFileRO`, `ReadFile`, `ListDirectory`, `TerminateSession`, and `ResetSessions`.
pub(all) struct FtpServer {
active_session : Byte
mut open_file_idx : Int32
files : Array[FtpVirtualFile]
} derive(Eq, Debug)
///|
pub extend FtpServer with Eq::{not_equal, equal}
///|
pub extend FtpServer with @debug.Debug::{to_repr}
///|
/// Create a new `FtpServer` populated with virtual files.
pub fn FtpServer::new(
files : Array[(String, Bytes)],
session_id~ : Byte = b'\x01',
) -> FtpServer {
let vf : Array[FtpVirtualFile] = []
for pair in files {
vf.push({ path: pair.0, content: pair.1, })
}
{ active_session: session_id, open_file_idx: -1, files: vf, }
}
///|
fn bytes_to_ascii_path(data : Bytes) -> String {
let sb = StringBuilder()
for i = 0; i < data.length(); i = i + 1 {
if data[i] == b'\x00' {
break
}
sb.write_char(Int32::unsafe_to_char(data[i].to_int() & 0xFF))
}
sb.to_string()
}
///|
/// Process an incoming `FtpPacket` request and return the corresponding `Ack` or `Nak` `FtpPacket`.
pub fn FtpServer::handle_packet(self : FtpServer, req : FtpPacket) -> FtpPacket {
let next_seq = (req.seq_number + 1U) & 0xFFFFU
match req.opcode {
OpenFileRO => {
let path = bytes_to_ascii_path(req.data)
for i = 0; i < self.files.length(); i = i + 1 {
if self.files[i].path == path {
self.open_file_idx = i
let flen = self.files[i].content.length().reinterpret_as_uint()
let size_bytes : Array[Byte] = [
(flen & 0xFFU).to_byte(),
((flen >> 8) & 0xFFU).to_byte(),
((flen >> 16) & 0xFFU).to_byte(),
((flen >> 24) & 0xFFU).to_byte(),
]
return {
seq_number: next_seq,
session: self.active_session,
opcode: Ack,
req_opcode: OpenFileRO,
burst_complete: false,
offset: 0U,
data: Bytes::from_array(size_bytes),
}
}
}
{
seq_number: next_seq,
session: b'\x00',
opcode: Nak,
req_opcode: OpenFileRO,
burst_complete: false,
offset: 0U,
data: b"\x0A", // FileNotFound (10)
}
}
ReadFile => {
if req.session != self.active_session ||
self.open_file_idx < 0 ||
self.open_file_idx >= self.files.length() {
return {
seq_number: next_seq,
session: req.session,
opcode: Nak,
req_opcode: ReadFile,
burst_complete: false,
offset: req.offset,
data: b"\x03", // InvalidSession (3)
}
}
let content = self.files[self.open_file_idx].content
let start = req.offset.reinterpret_as_int()
if start < 0 || start >= content.length() {
return {
seq_number: next_seq,
session: self.active_session,
opcode: Nak,
req_opcode: ReadFile,
burst_complete: false,
offset: req.offset,
data: b"\x06", // EOF (6)
}
}
let max_chunk = 64
let end = if start + max_chunk < content.length() {
start + max_chunk
} else {
content.length()
}
let chunk : Array[Byte] = Array(capacity=end - start)
for i = start; i < end; i = i + 1 {
chunk.push(content[i])
}
{
seq_number: next_seq,
session: self.active_session,
opcode: Ack,
req_opcode: ReadFile,
burst_complete: end >= content.length(),
offset: req.offset,
data: Bytes::from_array(chunk),
}
}
ListDirectory => {
let out : Array[Byte] = []
for f in self.files {
let entry_str = "F" + f.path
for ch in entry_str {
out.push((ch.to_int() & 0xFF).to_byte())
}
out.push(b'\x00')
}
{
seq_number: next_seq,
session: b'\x00',
opcode: Ack,
req_opcode: ListDirectory,
burst_complete: true,
offset: req.offset,
data: Bytes::from_array(out),
}
}
TerminateSession | ResetSessions => {
self.open_file_idx = -1
{
seq_number: next_seq,
session: b'\x00',
opcode: Ack,
req_opcode: req.opcode,
burst_complete: false,
offset: 0U,
data: b"",
}
}
_ =>
{
seq_number: next_seq,
session: req.session,
opcode: Nak,
req_opcode: req.opcode,
burst_complete: false,
offset: req.offset,
data: b"\x05", // UnknownCommand
}
}
}