pamoja_ffi/
mavlink_protocol.rs

1//! The C ABI for the MAVLink service protocols: mission transfer, commands, and offboard
2//! setpoints.
3//!
4//! A frame carries one message; a real exchange with an autopilot is a sequence of them with
5//! rules about order, matching, and retransmission. The machines here hold those rules and
6//! nothing else: no IO, no timers, no allocation beyond the handle. A caller feeds each one
7//! the frames off its link and sends back what the machine hands it, applying its own timing
8//! policy for timeouts and retransmission.
9//!
10//! Every machine takes a whole frame and answers with a whole frame, so the payload decoding,
11//! message-id dispatch, and reply encoding happen once, here, rather than in each caller. A
12//! frame a machine does not handle is reported as ignored rather than as an error, so one
13//! link's traffic can be routed through several machines in turn.
14
15use pamoja_mavlink::dialect::{
16    Message, MissionItemInt, SetPositionTargetGlobalInt, SetPositionTargetLocalNed,
17};
18use pamoja_mavlink::protocol::{
19    AckOutcome, CommandProtocol, MissionReceiver, MissionSender, ReceiverAction, ReceiverStep,
20    SenderStep, TypeMask, MAX_RETRIES,
21};
22use pamoja_mavlink::{dialect, Frame, Header};
23
24use crate::mavlink::{status_of, PamojaMavlinkFrame, PamojaMavlinkHeader};
25use crate::mavlink_schema::PamojaMavlinkMessage;
26use crate::{read_bytes, set_last_error, PamojaStatus};
27
28/// The number of times a request is retransmitted before a transfer is abandoned, as the
29/// mission protocol recommends.
30pub const PAMOJA_MAVLINK_MAX_RETRIES: u8 = 5;
31
32// The header carries this as a literal, because a generated macro cannot name a Rust
33// constant. This holds it to what the crate says.
34const _: () = assert!(PAMOJA_MAVLINK_MAX_RETRIES == MAX_RETRIES);
35
36/// The frame was not one this machine handles; nothing was produced.
37pub const PAMOJA_MAVLINK_STEP_IGNORED: u32 = 0;
38
39/// A mission receiver answered with a request for the next item.
40pub const PAMOJA_MAVLINK_RECEIVER_REQUEST: u32 = 1;
41/// A mission receiver answered with the acknowledgment that ends the transfer.
42pub const PAMOJA_MAVLINK_RECEIVER_ACK: u32 = 2;
43
44/// A mission sender answered with a frame to send.
45pub const PAMOJA_MAVLINK_SENDER_REPLY: u32 = 1;
46/// A mission sender saw the receiver's acknowledgment; the transfer is over.
47pub const PAMOJA_MAVLINK_SENDER_FINISHED: u32 = 2;
48
49/// An acknowledgment was for a different command; keep waiting.
50pub const PAMOJA_MAVLINK_ACK_UNRELATED: u32 = 1;
51/// The command is still running; the value is the reported progress percent, or 255 when
52/// the autopilot does not report one.
53pub const PAMOJA_MAVLINK_ACK_IN_PROGRESS: u32 = 2;
54/// The command finished; the value is its `MAV_RESULT`.
55pub const PAMOJA_MAVLINK_ACK_FINAL: u32 = 3;
56
57/// Use the position fields of a setpoint.
58pub const PAMOJA_MAVLINK_TYPEMASK_POSITION: u32 = 1 << 0;
59/// Use the velocity fields of a setpoint.
60pub const PAMOJA_MAVLINK_TYPEMASK_VELOCITY: u32 = 1 << 1;
61/// Use the acceleration fields of a setpoint.
62pub const PAMOJA_MAVLINK_TYPEMASK_ACCELERATION: u32 = 1 << 2;
63/// Use the yaw field of a setpoint.
64pub const PAMOJA_MAVLINK_TYPEMASK_YAW: u32 = 1 << 3;
65/// Use the yaw rate field of a setpoint.
66pub const PAMOJA_MAVLINK_TYPEMASK_YAW_RATE: u32 = 1 << 4;
67/// Treat the acceleration fields as a force.
68pub const PAMOJA_MAVLINK_TYPEMASK_FORCE: u32 = 1 << 5;
69
70/// Writes a frame through an out-pointer, reporting a null pointer as an error.
71unsafe fn emit(frame: Frame, out_frame: *mut *mut PamojaMavlinkFrame) -> PamojaStatus {
72    if out_frame.is_null() {
73        set_last_error("out_frame must not be null".to_owned());
74        return PamojaStatus::InvalidArgument;
75    }
76    *out_frame = PamojaMavlinkFrame::into_handle(frame);
77    PamojaStatus::Ok
78}
79
80/// Requests a plan's items in order and collects them, ending with an acknowledgment.
81pub struct PamojaMavlinkMissionReceiver {
82    inner: MissionReceiver,
83}
84
85/// Creates a receiver for a plan from a target vehicle.
86///
87/// # Arguments
88///
89/// * `target_system` - the sending system's id.
90/// * `target_component` - the sending component's id.
91/// * `mission_type` - the `MAV_MISSION_TYPE` to transfer.
92///
93/// # Returns
94///
95/// A receiver the caller releases with [`pamoja_mavlink_mission_receiver_free`].
96#[no_mangle]
97pub extern "C" fn pamoja_mavlink_mission_receiver_new(
98    target_system: u8,
99    target_component: u8,
100    mission_type: u8,
101) -> *mut PamojaMavlinkMissionReceiver {
102    Box::into_raw(Box::new(PamojaMavlinkMissionReceiver {
103        inner: MissionReceiver::new(target_system, target_component, mission_type),
104    }))
105}
106
107/// Builds the frame that starts a download.
108///
109/// # Arguments
110///
111/// * `receiver` - the transfer.
112/// * `header` - the addressing fields to stamp on the frame.
113/// * `out_frame` - set to the `MISSION_REQUEST_LIST` frame, which the caller releases with
114///   `pamoja_mavlink_frame_free`.
115///
116/// # Returns
117///
118/// [`PamojaStatus::Ok`] on success.
119///
120/// # Errors
121///
122/// Returns [`PamojaStatus::InvalidArgument`] if either pointer is null.
123///
124/// # Safety
125///
126/// `receiver` must be a live receiver handle, and `out_frame` must point at writable storage
127/// for one pointer.
128#[no_mangle]
129pub unsafe extern "C" fn pamoja_mavlink_mission_receiver_request_list(
130    receiver: *const PamojaMavlinkMissionReceiver,
131    header: PamojaMavlinkHeader,
132    out_frame: *mut *mut PamojaMavlinkFrame,
133) -> PamojaStatus {
134    let Some(receiver) = receiver.as_ref() else {
135        set_last_error("receiver must not be null".to_owned());
136        return PamojaStatus::InvalidArgument;
137    };
138    match receiver.inner.request_list_frame(Header::from(header)) {
139        Ok(frame) => emit(frame, out_frame),
140        Err(error) => status_of(error),
141    }
142}
143
144/// Handles an incoming frame, if it is one this transfer is waiting for.
145///
146/// A `MISSION_COUNT` opens the transfer and a `MISSION_ITEM_INT` advances it. Any other
147/// message sets `out_kind` to [`PAMOJA_MAVLINK_STEP_IGNORED`] and produces nothing.
148///
149/// # Arguments
150///
151/// * `receiver` - the transfer.
152/// * `frame` - the frame off the link.
153/// * `header` - the addressing fields to stamp on the reply.
154/// * `out_kind` - set to [`PAMOJA_MAVLINK_RECEIVER_REQUEST`],
155///   [`PAMOJA_MAVLINK_RECEIVER_ACK`], or [`PAMOJA_MAVLINK_STEP_IGNORED`].
156/// * `out_accepted` - set to the `MISSION_ITEM_INT` the frame carried if it was the one
157///   expected next, as a message the caller reads by field name and releases with
158///   `pamoja_mavlink_message_free`, or to null.
159/// * `out_reply` - set to the frame to send back, which the caller releases with
160///   `pamoja_mavlink_frame_free`, or to null if the frame was ignored.
161///
162/// # Returns
163///
164/// [`PamojaStatus::Ok`] on success, including when the frame was ignored.
165///
166/// # Errors
167///
168/// Returns [`PamojaStatus::InvalidArgument`] if any pointer is null.
169///
170/// # Safety
171///
172/// `receiver` must be a live receiver handle, `frame` a live frame handle, and each out
173/// pointer must point at writable storage for its value.
174#[no_mangle]
175pub unsafe extern "C" fn pamoja_mavlink_mission_receiver_on_frame(
176    receiver: *mut PamojaMavlinkMissionReceiver,
177    frame: *const PamojaMavlinkFrame,
178    header: PamojaMavlinkHeader,
179    out_kind: *mut u32,
180    out_accepted: *mut *mut PamojaMavlinkMessage,
181    out_reply: *mut *mut PamojaMavlinkFrame,
182) -> PamojaStatus {
183    let Some(receiver) = receiver.as_mut() else {
184        set_last_error("receiver must not be null".to_owned());
185        return PamojaStatus::InvalidArgument;
186    };
187    let Some(frame) = frame.as_ref() else {
188        set_last_error("frame must not be null".to_owned());
189        return PamojaStatus::InvalidArgument;
190    };
191    if out_kind.is_null() || out_accepted.is_null() || out_reply.is_null() {
192        set_last_error("the output pointers must not be null".to_owned());
193        return PamojaStatus::InvalidArgument;
194    }
195    *out_kind = PAMOJA_MAVLINK_STEP_IGNORED;
196    *out_accepted = std::ptr::null_mut();
197    *out_reply = std::ptr::null_mut();
198
199    let step = match receiver.inner.on_frame(frame.frame(), Header::from(header)) {
200        Ok(Some(step)) => step,
201        Ok(None) => return PamojaStatus::Ok,
202        Err(error) => return status_of(error),
203    };
204    let ReceiverStep {
205        accepted,
206        action,
207        reply,
208    } = step;
209    *out_kind = match action {
210        ReceiverAction::Request(_) => PAMOJA_MAVLINK_RECEIVER_REQUEST,
211        ReceiverAction::Ack(_) => PAMOJA_MAVLINK_RECEIVER_ACK,
212    };
213    if let Some(item) = accepted {
214        let mut payload = [0u8; pamoja_mavlink::MAX_PAYLOAD];
215        let len = item.encode(&mut payload);
216        *out_accepted =
217            PamojaMavlinkMessage::from_typed(MissionItemInt::DESCRIPTOR, payload[..len].to_vec());
218    }
219    *out_reply = PamojaMavlinkFrame::into_handle(reply);
220    PamojaStatus::Ok
221}
222
223/// Reports whether the transfer has finished.
224///
225/// # Arguments
226///
227/// * `receiver` - the transfer.
228///
229/// # Returns
230///
231/// `1` once every item has been received and the acknowledgment produced, `0` otherwise or
232/// if `receiver` is null.
233///
234/// # Safety
235///
236/// `receiver` must be a live receiver handle or null.
237#[no_mangle]
238pub unsafe extern "C" fn pamoja_mavlink_mission_receiver_is_complete(
239    receiver: *const PamojaMavlinkMissionReceiver,
240) -> u8 {
241    receiver
242        .as_ref()
243        .map_or(0, |receiver| u8::from(receiver.inner.is_complete()))
244}
245
246/// Returns the next sequence number the receiver expects.
247///
248/// # Arguments
249///
250/// * `receiver` - the transfer.
251///
252/// # Returns
253///
254/// The expected sequence number, or `0` if `receiver` is null.
255///
256/// # Safety
257///
258/// `receiver` must be a live receiver handle or null.
259#[no_mangle]
260pub unsafe extern "C" fn pamoja_mavlink_mission_receiver_expected(
261    receiver: *const PamojaMavlinkMissionReceiver,
262) -> u16 {
263    receiver
264        .as_ref()
265        .map_or(0, |receiver| receiver.inner.expected())
266}
267
268/// Releases a receiver.
269///
270/// # Arguments
271///
272/// * `receiver` - the handle to release; null is ignored.
273///
274/// # Safety
275///
276/// `receiver` must have come from [`pamoja_mavlink_mission_receiver_new`] and must not be
277/// used afterwards.
278#[no_mangle]
279pub unsafe extern "C" fn pamoja_mavlink_mission_receiver_free(
280    receiver: *mut PamojaMavlinkMissionReceiver,
281) {
282    if !receiver.is_null() {
283        drop(Box::from_raw(receiver));
284    }
285}
286
287/// Holds a plan and answers a receiver's requests for its items.
288pub struct PamojaMavlinkMissionSender {
289    items: Vec<MissionItemInt>,
290    target_system: u8,
291    target_component: u8,
292    mission_type: u8,
293}
294
295impl PamojaMavlinkMissionSender {
296    /// Runs a query against the borrowed sender the engine defines.
297    fn with<R>(&self, query: impl FnOnce(&MissionSender<'_>) -> R) -> R {
298        query(&MissionSender::new(
299            &self.items,
300            self.target_system,
301            self.target_component,
302            self.mission_type,
303        ))
304    }
305}
306
307/// Creates a sender for a plan bound for a target vehicle, with no items yet.
308///
309/// # Arguments
310///
311/// * `target_system` - the receiving system's id.
312/// * `target_component` - the receiving component's id.
313/// * `mission_type` - the `MAV_MISSION_TYPE` of the plan.
314///
315/// # Returns
316///
317/// A sender the caller releases with [`pamoja_mavlink_mission_sender_free`].
318#[no_mangle]
319pub extern "C" fn pamoja_mavlink_mission_sender_new(
320    target_system: u8,
321    target_component: u8,
322    mission_type: u8,
323) -> *mut PamojaMavlinkMissionSender {
324    Box::into_raw(Box::new(PamojaMavlinkMissionSender {
325        items: Vec::new(),
326        target_system,
327        target_component,
328        mission_type,
329    }))
330}
331
332/// Appends an item to the plan.
333///
334/// The sender stamps the sequence number, target ids, and mission type onto each item as it
335/// is handed out, so the payload need only carry the item's content: its command, frame,
336/// position, and parameters. Build one by field name with the message schema for
337/// `MISSION_ITEM_INT` and pass its payload.
338///
339/// # Arguments
340///
341/// * `sender` - the plan to extend.
342/// * `payload` - a `MISSION_ITEM_INT` payload.
343/// * `payload_len` - the payload length in bytes.
344///
345/// # Returns
346///
347/// [`PamojaStatus::Ok`] on success.
348///
349/// # Errors
350///
351/// Returns [`PamojaStatus::InvalidArgument`] if a pointer is null, and
352/// [`PamojaStatus::Codec`] if the payload does not form an item.
353///
354/// # Safety
355///
356/// `sender` must be a live sender handle and `payload` must point at `payload_len` readable
357/// bytes.
358#[no_mangle]
359pub unsafe extern "C" fn pamoja_mavlink_mission_sender_add_item(
360    sender: *mut PamojaMavlinkMissionSender,
361    payload: *const u8,
362    payload_len: usize,
363) -> PamojaStatus {
364    let Some(sender) = sender.as_mut() else {
365        set_last_error("sender must not be null".to_owned());
366        return PamojaStatus::InvalidArgument;
367    };
368    let payload = match read_bytes(payload, payload_len) {
369        Ok(payload) => payload,
370        Err(status) => return status,
371    };
372    match MissionItemInt::decode(&payload) {
373        Ok(item) => {
374            sender.items.push(item);
375            PamojaStatus::Ok
376        }
377        Err(error) => status_of(error),
378    }
379}
380
381/// Returns the number of items in the plan.
382///
383/// # Arguments
384///
385/// * `sender` - the plan.
386///
387/// # Returns
388///
389/// The item count, or `0` if `sender` is null.
390///
391/// # Safety
392///
393/// `sender` must be a live sender handle or null.
394#[no_mangle]
395pub unsafe extern "C" fn pamoja_mavlink_mission_sender_len(
396    sender: *const PamojaMavlinkMissionSender,
397) -> u16 {
398    sender
399        .as_ref()
400        .map_or(0, |sender| sender.with(|plan| plan.len()))
401}
402
403/// Builds the frame that opens an upload.
404///
405/// # Arguments
406///
407/// * `sender` - the plan.
408/// * `header` - the addressing fields to stamp on the frame.
409/// * `out_frame` - set to the `MISSION_COUNT` frame, which the caller releases with
410///   `pamoja_mavlink_frame_free`.
411///
412/// # Returns
413///
414/// [`PamojaStatus::Ok`] on success.
415///
416/// # Errors
417///
418/// Returns [`PamojaStatus::InvalidArgument`] if either pointer is null.
419///
420/// # Safety
421///
422/// `sender` must be a live sender handle, and `out_frame` must point at writable storage for
423/// one pointer.
424#[no_mangle]
425pub unsafe extern "C" fn pamoja_mavlink_mission_sender_count(
426    sender: *const PamojaMavlinkMissionSender,
427    header: PamojaMavlinkHeader,
428    out_frame: *mut *mut PamojaMavlinkFrame,
429) -> PamojaStatus {
430    let Some(sender) = sender.as_ref() else {
431        set_last_error("sender must not be null".to_owned());
432        return PamojaStatus::InvalidArgument;
433    };
434    match sender.with(|plan| plan.count_frame(Header::from(header))) {
435        Ok(frame) => emit(frame, out_frame),
436        Err(error) => status_of(error),
437    }
438}
439
440/// Handles an incoming frame, if it is one this transfer answers.
441///
442/// A `MISSION_REQUEST_LIST` is answered with the count, a `MISSION_REQUEST_INT` (or the
443/// older `MISSION_REQUEST`) with the item asked for, and a request past the end of the plan
444/// with a `MISSION_ACK` reporting an invalid sequence. A `MISSION_ACK` from the receiver
445/// ends the transfer. Any other message sets `out_kind` to [`PAMOJA_MAVLINK_STEP_IGNORED`].
446///
447/// # Arguments
448///
449/// * `sender` - the plan.
450/// * `frame` - the frame off the link.
451/// * `header` - the addressing fields to stamp on the reply.
452/// * `out_kind` - set to [`PAMOJA_MAVLINK_SENDER_REPLY`],
453///   [`PAMOJA_MAVLINK_SENDER_FINISHED`], or [`PAMOJA_MAVLINK_STEP_IGNORED`].
454/// * `out_result` - set to the receiver's `MAV_MISSION_RESULT` when the transfer finished.
455/// * `out_reply` - set to the frame to send back, which the caller releases with
456///   `pamoja_mavlink_frame_free`, or to null if there is nothing to send.
457///
458/// # Returns
459///
460/// [`PamojaStatus::Ok`] on success, including when the frame was ignored.
461///
462/// # Errors
463///
464/// Returns [`PamojaStatus::InvalidArgument`] if any pointer is null.
465///
466/// # Safety
467///
468/// `sender` must be a live sender handle, `frame` a live frame handle, and each out pointer
469/// must point at writable storage for its value.
470#[no_mangle]
471pub unsafe extern "C" fn pamoja_mavlink_mission_sender_on_frame(
472    sender: *const PamojaMavlinkMissionSender,
473    frame: *const PamojaMavlinkFrame,
474    header: PamojaMavlinkHeader,
475    out_kind: *mut u32,
476    out_result: *mut u8,
477    out_reply: *mut *mut PamojaMavlinkFrame,
478) -> PamojaStatus {
479    let Some(sender) = sender.as_ref() else {
480        set_last_error("sender must not be null".to_owned());
481        return PamojaStatus::InvalidArgument;
482    };
483    let Some(frame) = frame.as_ref() else {
484        set_last_error("frame must not be null".to_owned());
485        return PamojaStatus::InvalidArgument;
486    };
487    if out_kind.is_null() || out_result.is_null() || out_reply.is_null() {
488        set_last_error("the output pointers must not be null".to_owned());
489        return PamojaStatus::InvalidArgument;
490    }
491    *out_kind = PAMOJA_MAVLINK_STEP_IGNORED;
492    *out_result = 0;
493    *out_reply = std::ptr::null_mut();
494
495    match sender.with(|plan| plan.on_frame(frame.frame(), Header::from(header))) {
496        Ok(Some(SenderStep::Reply(reply))) => {
497            *out_kind = PAMOJA_MAVLINK_SENDER_REPLY;
498            *out_reply = PamojaMavlinkFrame::into_handle(reply);
499            PamojaStatus::Ok
500        }
501        Ok(Some(SenderStep::Finished(result))) => {
502            *out_kind = PAMOJA_MAVLINK_SENDER_FINISHED;
503            *out_result = result;
504            PamojaStatus::Ok
505        }
506        Ok(None) => PamojaStatus::Ok,
507        Err(error) => status_of(error),
508    }
509}
510
511/// Releases a sender.
512///
513/// # Arguments
514///
515/// * `sender` - the handle to release; null is ignored.
516///
517/// # Safety
518///
519/// `sender` must have come from [`pamoja_mavlink_mission_sender_new`] and must not be used
520/// afterwards.
521#[no_mangle]
522pub unsafe extern "C" fn pamoja_mavlink_mission_sender_free(
523    sender: *mut PamojaMavlinkMissionSender,
524) {
525    if !sender.is_null() {
526        drop(Box::from_raw(sender));
527    }
528}
529
530/// Tracks one command awaiting its acknowledgment.
531pub struct PamojaMavlinkCommand {
532    inner: CommandProtocol,
533}
534
535/// Starts tracking a command.
536///
537/// # Arguments
538///
539/// * `command` - the `MAV_CMD` id being sent.
540/// * `max_retries` - how many times the command may be resent after a timeout before the
541///   caller gives up; [`PAMOJA_MAVLINK_MAX_RETRIES`] is the usual choice.
542///
543/// # Returns
544///
545/// A tracker the caller releases with [`pamoja_mavlink_command_free`].
546#[no_mangle]
547pub extern "C" fn pamoja_mavlink_command_new(
548    command: u16,
549    max_retries: u8,
550) -> *mut PamojaMavlinkCommand {
551    Box::into_raw(Box::new(PamojaMavlinkCommand {
552        inner: CommandProtocol::new(command, max_retries),
553    }))
554}
555
556/// Returns the command id being tracked.
557///
558/// # Arguments
559///
560/// * `command` - the tracker.
561///
562/// # Returns
563///
564/// The command id, or `0` if `command` is null.
565///
566/// # Safety
567///
568/// `command` must be a live tracker handle or null.
569#[no_mangle]
570pub unsafe extern "C" fn pamoja_mavlink_command_id(command: *const PamojaMavlinkCommand) -> u16 {
571    command
572        .as_ref()
573        .map_or(0, |command| command.inner.command())
574}
575
576/// Returns the `confirmation` count to stamp on the command being sent.
577///
578/// It is zero for the first transmission and increments on each retransmission, which is
579/// how an autopilot distinguishes a resend from a new command.
580///
581/// # Arguments
582///
583/// * `command` - the tracker.
584///
585/// # Returns
586///
587/// The current confirmation count, or `0` if `command` is null.
588///
589/// # Safety
590///
591/// `command` must be a live tracker handle or null.
592#[no_mangle]
593pub unsafe extern "C" fn pamoja_mavlink_command_confirmation(
594    command: *const PamojaMavlinkCommand,
595) -> u8 {
596    command
597        .as_ref()
598        .map_or(0, |command| command.inner.confirmation())
599}
600
601/// Classifies an incoming frame against the command in flight.
602///
603/// # Arguments
604///
605/// * `command` - the tracker.
606/// * `frame` - the frame off the link.
607/// * `out_kind` - set to [`PAMOJA_MAVLINK_ACK_UNRELATED`], [`PAMOJA_MAVLINK_ACK_IN_PROGRESS`],
608///   [`PAMOJA_MAVLINK_ACK_FINAL`], or [`PAMOJA_MAVLINK_STEP_IGNORED`] if the frame is not a
609///   `COMMAND_ACK`.
610/// * `out_value` - set to the progress percent when in progress, or the `MAV_RESULT` when
611///   final.
612///
613/// # Returns
614///
615/// [`PamojaStatus::Ok`] on success, including when the frame was ignored.
616///
617/// # Errors
618///
619/// Returns [`PamojaStatus::InvalidArgument`] if any pointer is null.
620///
621/// # Safety
622///
623/// `command` must be a live tracker handle, `frame` a live frame handle, and each out
624/// pointer must point at writable storage for its value.
625#[no_mangle]
626pub unsafe extern "C" fn pamoja_mavlink_command_on_frame(
627    command: *const PamojaMavlinkCommand,
628    frame: *const PamojaMavlinkFrame,
629    out_kind: *mut u32,
630    out_value: *mut u8,
631) -> PamojaStatus {
632    let Some(command) = command.as_ref() else {
633        set_last_error("command must not be null".to_owned());
634        return PamojaStatus::InvalidArgument;
635    };
636    let Some(frame) = frame.as_ref() else {
637        set_last_error("frame must not be null".to_owned());
638        return PamojaStatus::InvalidArgument;
639    };
640    if out_kind.is_null() || out_value.is_null() {
641        set_last_error("the output pointers must not be null".to_owned());
642        return PamojaStatus::InvalidArgument;
643    }
644    *out_kind = PAMOJA_MAVLINK_STEP_IGNORED;
645    *out_value = 0;
646    match command.inner.on_frame(frame.frame()) {
647        Ok(Some(AckOutcome::Unrelated)) => *out_kind = PAMOJA_MAVLINK_ACK_UNRELATED,
648        Ok(Some(AckOutcome::InProgress(progress))) => {
649            *out_kind = PAMOJA_MAVLINK_ACK_IN_PROGRESS;
650            *out_value = progress;
651        }
652        Ok(Some(AckOutcome::Final(result))) => {
653            *out_kind = PAMOJA_MAVLINK_ACK_FINAL;
654            *out_value = result;
655        }
656        Ok(None) => {}
657        Err(error) => return status_of(error),
658    }
659    PamojaStatus::Ok
660}
661
662/// Records a timeout and reports whether the command may be resent.
663///
664/// On a resend the `confirmation` count is incremented, so the next call to
665/// [`pamoja_mavlink_command_confirmation`] stamps the new value.
666///
667/// # Arguments
668///
669/// * `command` - the tracker.
670/// * `out_confirmation` - set to the new confirmation count when a resend is allowed.
671///
672/// # Returns
673///
674/// `1` if a retry remains and the command should be resent, `0` once the retry budget is
675/// exhausted or if a pointer is null.
676///
677/// # Safety
678///
679/// `command` must be a live tracker handle, and `out_confirmation` must point at writable
680/// storage for one byte.
681#[no_mangle]
682pub unsafe extern "C" fn pamoja_mavlink_command_on_timeout(
683    command: *mut PamojaMavlinkCommand,
684    out_confirmation: *mut u8,
685) -> u8 {
686    let Some(command) = command.as_mut() else {
687        return 0;
688    };
689    if out_confirmation.is_null() {
690        return 0;
691    }
692    match command.inner.on_timeout() {
693        Some(confirmation) => {
694            *out_confirmation = confirmation;
695            1
696        }
697        None => 0,
698    }
699}
700
701/// Releases a command tracker.
702///
703/// # Arguments
704///
705/// * `command` - the handle to release; null is ignored.
706///
707/// # Safety
708///
709/// `command` must have come from [`pamoja_mavlink_command_new`] and must not be used
710/// afterwards.
711#[no_mangle]
712pub unsafe extern "C" fn pamoja_mavlink_command_free(command: *mut PamojaMavlinkCommand) {
713    if !command.is_null() {
714        drop(Box::from_raw(command));
715    }
716}
717
718/// Builds a setpoint `type_mask` from the fields to use.
719///
720/// A setpoint carries position, velocity, acceleration, yaw, and yaw rate together; the mask
721/// says which of them the autopilot should act on. Fields left out of `flags` are ignored.
722///
723/// # Arguments
724///
725/// * `flags` - a bitwise-or of the `PAMOJA_MAVLINK_TYPEMASK_*` flags.
726///
727/// # Returns
728///
729/// The mask, as the `type_mask` field of a setpoint carries it.
730#[no_mangle]
731pub extern "C" fn pamoja_mavlink_offboard_type_mask(flags: u32) -> u16 {
732    let mut mask = TypeMask::ignore_all();
733    if flags & PAMOJA_MAVLINK_TYPEMASK_POSITION != 0 {
734        mask = mask.use_position();
735    }
736    if flags & PAMOJA_MAVLINK_TYPEMASK_VELOCITY != 0 {
737        mask = mask.use_velocity();
738    }
739    if flags & PAMOJA_MAVLINK_TYPEMASK_ACCELERATION != 0 {
740        mask = mask.use_acceleration();
741    }
742    if flags & PAMOJA_MAVLINK_TYPEMASK_YAW != 0 {
743        mask = mask.use_yaw();
744    }
745    if flags & PAMOJA_MAVLINK_TYPEMASK_YAW_RATE != 0 {
746        mask = mask.use_yaw_rate();
747    }
748    if flags & PAMOJA_MAVLINK_TYPEMASK_FORCE != 0 {
749        mask = mask.force();
750    }
751    mask.bits()
752}
753
754/// Builds a local-frame position setpoint frame.
755///
756/// # Arguments
757///
758/// * `header` - the addressing fields to stamp on the frame.
759/// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
760/// * `coordinate_frame` - the `MAV_FRAME` of the setpoint.
761/// * `target_system` - the target system id.
762/// * `target_component` - the target component id.
763/// * `x`, `y`, `z` - the position, in meters in the chosen frame.
764/// * `out_frame` - set to the `SET_POSITION_TARGET_LOCAL_NED` frame, which the caller
765///   releases with `pamoja_mavlink_frame_free`.
766///
767/// # Returns
768///
769/// [`PamojaStatus::Ok`] on success.
770///
771/// # Errors
772///
773/// Returns [`PamojaStatus::InvalidArgument`] if `out_frame` is null.
774///
775/// # Safety
776///
777/// `out_frame` must point at writable storage for one pointer.
778#[no_mangle]
779#[allow(clippy::too_many_arguments)]
780pub unsafe extern "C" fn pamoja_mavlink_offboard_local_position(
781    header: PamojaMavlinkHeader,
782    time_boot_ms: u32,
783    coordinate_frame: u8,
784    target_system: u8,
785    target_component: u8,
786    x: f32,
787    y: f32,
788    z: f32,
789    out_frame: *mut *mut PamojaMavlinkFrame,
790) -> PamojaStatus {
791    let setpoint = SetPositionTargetLocalNed::position(
792        time_boot_ms,
793        coordinate_frame,
794        target_system,
795        target_component,
796        x,
797        y,
798        z,
799    );
800    match dialect::encode_message(Header::from(header), &setpoint) {
801        Ok(frame) => emit(frame, out_frame),
802        Err(error) => status_of(error),
803    }
804}
805
806/// Builds a local-frame velocity setpoint frame.
807///
808/// # Arguments
809///
810/// * `header` - the addressing fields to stamp on the frame.
811/// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
812/// * `coordinate_frame` - the `MAV_FRAME` of the setpoint.
813/// * `target_system` - the target system id.
814/// * `target_component` - the target component id.
815/// * `vx`, `vy`, `vz` - the velocity, in meters per second in the chosen frame.
816/// * `out_frame` - set to the `SET_POSITION_TARGET_LOCAL_NED` frame, which the caller
817///   releases with `pamoja_mavlink_frame_free`.
818///
819/// # Returns
820///
821/// [`PamojaStatus::Ok`] on success.
822///
823/// # Errors
824///
825/// Returns [`PamojaStatus::InvalidArgument`] if `out_frame` is null.
826///
827/// # Safety
828///
829/// `out_frame` must point at writable storage for one pointer.
830#[no_mangle]
831#[allow(clippy::too_many_arguments)]
832pub unsafe extern "C" fn pamoja_mavlink_offboard_local_velocity(
833    header: PamojaMavlinkHeader,
834    time_boot_ms: u32,
835    coordinate_frame: u8,
836    target_system: u8,
837    target_component: u8,
838    vx: f32,
839    vy: f32,
840    vz: f32,
841    out_frame: *mut *mut PamojaMavlinkFrame,
842) -> PamojaStatus {
843    let setpoint = SetPositionTargetLocalNed::velocity(
844        time_boot_ms,
845        coordinate_frame,
846        target_system,
847        target_component,
848        vx,
849        vy,
850        vz,
851    );
852    match dialect::encode_message(Header::from(header), &setpoint) {
853        Ok(frame) => emit(frame, out_frame),
854        Err(error) => status_of(error),
855    }
856}
857
858/// Builds a global-frame position setpoint frame.
859///
860/// # Arguments
861///
862/// * `header` - the addressing fields to stamp on the frame.
863/// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
864/// * `coordinate_frame` - the `MAV_FRAME` of the setpoint.
865/// * `target_system` - the target system id.
866/// * `target_component` - the target component id.
867/// * `lat_int`, `lon_int` - the latitude and longitude, in degrees times ten million.
868/// * `alt` - the altitude, in meters.
869/// * `out_frame` - set to the `SET_POSITION_TARGET_GLOBAL_INT` frame, which the caller
870///   releases with `pamoja_mavlink_frame_free`.
871///
872/// # Returns
873///
874/// [`PamojaStatus::Ok`] on success.
875///
876/// # Errors
877///
878/// Returns [`PamojaStatus::InvalidArgument`] if `out_frame` is null.
879///
880/// # Safety
881///
882/// `out_frame` must point at writable storage for one pointer.
883#[no_mangle]
884#[allow(clippy::too_many_arguments)]
885pub unsafe extern "C" fn pamoja_mavlink_offboard_global_position(
886    header: PamojaMavlinkHeader,
887    time_boot_ms: u32,
888    coordinate_frame: u8,
889    target_system: u8,
890    target_component: u8,
891    lat_int: i32,
892    lon_int: i32,
893    alt: f32,
894    out_frame: *mut *mut PamojaMavlinkFrame,
895) -> PamojaStatus {
896    let setpoint = SetPositionTargetGlobalInt::position(
897        time_boot_ms,
898        coordinate_frame,
899        target_system,
900        target_component,
901        lat_int,
902        lon_int,
903        alt,
904    );
905    match dialect::encode_message(Header::from(header), &setpoint) {
906        Ok(frame) => emit(frame, out_frame),
907        Err(error) => status_of(error),
908    }
909}
910
911#[cfg(test)]
912mod tests {
913    use super::*;
914    use crate::mavlink::{pamoja_mavlink_frame_free, pamoja_mavlink_frame_message_id};
915    use crate::mavlink_schema::pamoja_mavlink_message_free;
916    use pamoja_mavlink::dialect::{mav_cmd, mav_mission_result, mav_result, CommandAck};
917
918    const VEHICLE: PamojaMavlinkHeader = PamojaMavlinkHeader {
919        system_id: 1,
920        component_id: 1,
921        sequence: 0,
922    };
923    const STATION: PamojaMavlinkHeader = PamojaMavlinkHeader {
924        system_id: 255,
925        component_id: 190,
926        sequence: 0,
927    };
928
929    fn item_payload(command: u16, z: f32) -> Vec<u8> {
930        let item = MissionItemInt {
931            command,
932            z,
933            ..MissionItemInt::zeroed()
934        };
935        let mut payload = [0u8; pamoja_mavlink::MAX_PAYLOAD];
936        let len = item.encode(&mut payload);
937        payload[..len].to_vec()
938    }
939
940    #[test]
941    fn a_whole_upload_runs_across_the_boundary() {
942        unsafe {
943            let sender = pamoja_mavlink_mission_sender_new(1, 1, 0);
944            for (command, z) in [(mav_cmd::NAV_TAKEOFF, 20.0), (mav_cmd::NAV_WAYPOINT, 50.0)] {
945                let payload = item_payload(command, z);
946                assert_eq!(
947                    pamoja_mavlink_mission_sender_add_item(sender, payload.as_ptr(), payload.len()),
948                    PamojaStatus::Ok
949                );
950            }
951            assert_eq!(pamoja_mavlink_mission_sender_len(sender), 2);
952
953            let receiver = pamoja_mavlink_mission_receiver_new(255, 190, 0);
954            let mut opened = std::ptr::null_mut();
955            assert_eq!(
956                pamoja_mavlink_mission_receiver_request_list(receiver, STATION, &mut opened),
957                PamojaStatus::Ok
958            );
959
960            let mut kind = 0;
961            let mut result = 0;
962            let mut from_vehicle = std::ptr::null_mut();
963            assert_eq!(
964                pamoja_mavlink_mission_sender_on_frame(
965                    sender,
966                    opened,
967                    VEHICLE,
968                    &mut kind,
969                    &mut result,
970                    &mut from_vehicle
971                ),
972                PamojaStatus::Ok
973            );
974            pamoja_mavlink_frame_free(opened);
975            assert_eq!(kind, PAMOJA_MAVLINK_SENDER_REPLY);
976            assert_eq!(pamoja_mavlink_frame_message_id(from_vehicle), 44);
977
978            let mut accepted_items = 0;
979            loop {
980                let mut accepted = std::ptr::null_mut();
981                let mut reply = std::ptr::null_mut();
982                assert_eq!(
983                    pamoja_mavlink_mission_receiver_on_frame(
984                        receiver,
985                        from_vehicle,
986                        STATION,
987                        &mut kind,
988                        &mut accepted,
989                        &mut reply
990                    ),
991                    PamojaStatus::Ok
992                );
993                pamoja_mavlink_frame_free(from_vehicle);
994                assert_ne!(kind, PAMOJA_MAVLINK_STEP_IGNORED);
995                if !accepted.is_null() {
996                    accepted_items += 1;
997                    pamoja_mavlink_message_free(accepted);
998                }
999
1000                let mut next = std::ptr::null_mut();
1001                assert_eq!(
1002                    pamoja_mavlink_mission_sender_on_frame(
1003                        sender,
1004                        reply,
1005                        VEHICLE,
1006                        &mut kind,
1007                        &mut result,
1008                        &mut next
1009                    ),
1010                    PamojaStatus::Ok
1011                );
1012                pamoja_mavlink_frame_free(reply);
1013                if kind == PAMOJA_MAVLINK_SENDER_FINISHED {
1014                    assert_eq!(result, mav_mission_result::ACCEPTED);
1015                    break;
1016                }
1017                assert_eq!(kind, PAMOJA_MAVLINK_SENDER_REPLY);
1018                from_vehicle = next;
1019            }
1020            assert_eq!(accepted_items, 2);
1021            assert_eq!(pamoja_mavlink_mission_receiver_is_complete(receiver), 1);
1022
1023            pamoja_mavlink_mission_receiver_free(receiver);
1024            pamoja_mavlink_mission_sender_free(sender);
1025        }
1026    }
1027
1028    #[test]
1029    fn a_command_is_matched_to_its_acknowledgment_and_retried() {
1030        unsafe {
1031            let arm = pamoja_mavlink_command_new(mav_cmd::COMPONENT_ARM_DISARM, 2);
1032            assert_eq!(
1033                pamoja_mavlink_command_id(arm),
1034                mav_cmd::COMPONENT_ARM_DISARM
1035            );
1036            assert_eq!(pamoja_mavlink_command_confirmation(arm), 0);
1037
1038            let ack = CommandAck {
1039                command: mav_cmd::COMPONENT_ARM_DISARM,
1040                result: mav_result::ACCEPTED,
1041                ..CommandAck::zeroed()
1042            };
1043            let frame = PamojaMavlinkFrame::into_handle(
1044                dialect::encode_message(Header::from(VEHICLE), &ack).unwrap(),
1045            );
1046            let mut kind = 0;
1047            let mut value = 0;
1048            assert_eq!(
1049                pamoja_mavlink_command_on_frame(arm, frame, &mut kind, &mut value),
1050                PamojaStatus::Ok
1051            );
1052            assert_eq!(kind, PAMOJA_MAVLINK_ACK_FINAL);
1053            assert_eq!(value, mav_result::ACCEPTED);
1054            pamoja_mavlink_frame_free(frame);
1055
1056            // Two retries, then the budget is spent.
1057            let mut confirmation = 0;
1058            assert_eq!(pamoja_mavlink_command_on_timeout(arm, &mut confirmation), 1);
1059            assert_eq!(confirmation, 1);
1060            assert_eq!(pamoja_mavlink_command_on_timeout(arm, &mut confirmation), 1);
1061            assert_eq!(confirmation, 2);
1062            assert_eq!(pamoja_mavlink_command_on_timeout(arm, &mut confirmation), 0);
1063            pamoja_mavlink_command_free(arm);
1064        }
1065    }
1066
1067    #[test]
1068    fn a_setpoint_goes_out_as_the_right_message() {
1069        unsafe {
1070            let mut frame = std::ptr::null_mut();
1071            assert_eq!(
1072                pamoja_mavlink_offboard_local_velocity(
1073                    STATION, 1_000, 1, 1, 1, 0.5, 0.0, 0.0, &mut frame
1074                ),
1075                PamojaStatus::Ok
1076            );
1077            assert_eq!(pamoja_mavlink_frame_message_id(frame), 84);
1078            pamoja_mavlink_frame_free(frame);
1079
1080            assert_eq!(
1081                pamoja_mavlink_offboard_global_position(
1082                    STATION,
1083                    1_000,
1084                    6,
1085                    1,
1086                    1,
1087                    -338_567_800,
1088                    1_512_153_000,
1089                    50.0,
1090                    &mut frame
1091                ),
1092                PamojaStatus::Ok
1093            );
1094            assert_eq!(pamoja_mavlink_frame_message_id(frame), 86);
1095            pamoja_mavlink_frame_free(frame);
1096
1097            let mask = pamoja_mavlink_offboard_type_mask(
1098                PAMOJA_MAVLINK_TYPEMASK_VELOCITY | PAMOJA_MAVLINK_TYPEMASK_YAW_RATE,
1099            );
1100            assert_eq!(
1101                mask,
1102                TypeMask::ignore_all().use_velocity().use_yaw_rate().bits()
1103            );
1104        }
1105    }
1106}
pamoja_ffi/mavlink_protocol.rs

pamoja_ffi/
mavlink_protocol.rs
