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

pamoja_ffi/
mavlink_protocol.rs
