Skip to main content

ekf2_sys/
lib.rs

1//! Raw FFI bindings for the PX4 EKF2 C++ library.
2//!
3//! This crate is a low-level `unsafe` interface. Most users should use the
4//! safe [`ekf2`](https://docs.rs/ekf2) crate instead.
5//!
6//! # Feature flags
7//!
8//! Each sensor type and integration option is guarded by a Cargo feature. The
9//! sensor features map 1-to-1 to the corresponding `CONFIG_EKF2_*` C
10//! preprocessor define:
11//!
12//! | Feature            | Default | C define                    |
13//! |--------------------|:-------:|-----------------------------|
14//! | `gnss`             | yes     | `CONFIG_EKF2_GNSS`          |
15//! | `gnss-yaw`         | no      | `CONFIG_EKF2_GNSS_YAW`      |
16//! | `magnetometer`     | yes     | `CONFIG_EKF2_MAGNETOMETER`  |
17//! | `barometer`        | yes     | `CONFIG_EKF2_BAROMETER`     |
18//! | `baro-compensation`| no      | `CONFIG_EKF2_BARO_COMPENSATION` |
19//! | `airspeed`         | no      | `CONFIG_EKF2_AIRSPEED`      |
20//! | `range-finder`     | no      | `CONFIG_EKF2_RANGE_FINDER`  |
21//! | `optical-flow`     | no      | `CONFIG_EKF2_OPTICAL_FLOW`  |
22//! | `external-vision`  | no      | `CONFIG_EKF2_EXTERNAL_VISION` |
23//! | `aux-vel`          | no      | `CONFIG_EKF2_AUXVEL`        |
24//! | `drag-fusion`      | no      | `CONFIG_EKF2_DRAG_FUSION`   |
25//! | `wind`             | no      | `CONFIG_EKF2_WIND`          |
26//! | `terrain`          | no      | `CONFIG_EKF2_TERRAIN`       |
27//! | `gravity-fusion`   | no      | `CONFIG_EKF2_GRAVITY_FUSION`|
28//! | `sideslip`         | no      | `CONFIG_EKF2_SIDESLIP`      |
29//! | `logging`          | no      | -                           |
30//! | `c-stubs`          | no      | -                           |
31//!
32//! `range-finder` and `optical-flow` imply `terrain`. `drag-fusion` and
33//! `sideslip` imply `wind`; `airspeed` implies all three of those features.
34//!
35//! # Memory
36//!
37//! `ekf2_create_heap` uses the Rust global allocator through the exported
38//! `ekf2_rust_alloc` callbacks. `ekf2_create_heap_with_allocator` accepts an
39//! explicit callback table instead. C++ stores that table in the EKF object
40//! and uses it for all dynamic storage owned by the object.
41//!
42//! The `ekf2_create` variants place the EKF object in caller-provided storage;
43//! its internal dynamic storage still uses the selected callback table.
44
45#![no_std]
46#![allow(non_upper_case_globals)]
47#![allow(non_camel_case_types)]
48#![allow(non_snake_case)]
49#![allow(clippy::all)]
50
51extern crate alloc;
52
53use alloc::alloc::{
54    alloc as rust_alloc, alloc_zeroed as rust_alloc_zeroed, dealloc as rust_dealloc,
55};
56use core::alloc::Layout;
57
58// Generated bindings from ekf2_wrapper.h
59#[doc(hidden)]
60mod generated {
61    #![allow(rustdoc::broken_intra_doc_links)]
62
63    include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
64}
65
66#[doc(inline)]
67pub use generated::*;
68
69#[cfg(all(target_os = "none", feature = "c-stubs"))]
70mod stubs;
71
72/// C ABI allocator shim used by C++ glue code.
73#[no_mangle]
74pub unsafe extern "C" fn ekf2_rust_alloc(size: usize, align: usize) -> *mut u8 {
75    match Layout::from_size_align(size, align) {
76        Ok(layout) => unsafe { rust_alloc(layout) },
77        Err(_) => core::ptr::null_mut(),
78    }
79}
80
81/// C ABI zeroed allocator shim used by C++ glue code.
82#[no_mangle]
83pub unsafe extern "C" fn ekf2_rust_alloc_zeroed(size: usize, align: usize) -> *mut u8 {
84    match Layout::from_size_align(size, align) {
85        Ok(layout) => unsafe { rust_alloc_zeroed(layout) },
86        Err(_) => core::ptr::null_mut(),
87    }
88}
89
90/// C ABI deallocator shim used by C++ glue code.
91#[no_mangle]
92pub unsafe extern "C" fn ekf2_rust_dealloc(ptr: *mut u8, size: usize, align: usize) {
93    if ptr.is_null() {
94        return;
95    }
96    if let Ok(layout) = Layout::from_size_align(size, align) {
97        unsafe { rust_dealloc(ptr, layout) };
98    }
99}
100
101/// Active primary height sensor reference.
102///
103/// Matches PX4's `HeightSensor` enum (`common.h`).
104#[repr(u8)]
105#[derive(Debug, Clone, Copy, PartialEq, Eq)]
106pub enum HeightSensor {
107    Baro = 0,
108    Gnss = 1,
109    Range = 2,
110    Ev = 3,
111    Unknown = 4,
112}
113
114impl HeightSensor {
115    /// Convert from the raw `u8` returned by `ekf2_get_height_sensor_ref`.
116    #[inline]
117    pub fn from_raw(v: u8) -> Self {
118        match v {
119            0 => Self::Baro,
120            1 => Self::Gnss,
121            2 => Self::Range,
122            3 => Self::Ev,
123            _ => Self::Unknown,
124        }
125    }
126}
127
128/// WGS-84 origin used by the EKF for local↔global position conversion.
129#[derive(Debug, Clone, Copy, Default)]
130pub struct GlobalOrigin {
131    /// Timestamp when the origin was set, in microseconds.
132    pub time_us: u64,
133    /// Latitude in degrees.
134    pub lat: f64,
135    /// Longitude in degrees.
136    pub lon: f64,
137    /// Altitude above the WGS-84 ellipsoid, in meters.
138    pub alt: f32,
139}
140
141/// Position reference frame for external vision samples.
142///
143/// Matches PX4's `PositionFrame` enum (`common.h`).
144#[repr(u8)]
145#[derive(Debug, Clone, Copy, PartialEq, Eq)]
146pub enum PositionFrame {
147    /// NED (North-East-Down) local frame.
148    LocalFrameNed = 0,
149    /// FRD (Forward-Right-Down) local frame.
150    LocalFrameFrd = 1,
151}
152
153/// Velocity reference frame for external vision samples.
154///
155/// Matches PX4's `VelocityFrame` enum (`common.h`).
156#[repr(u8)]
157#[derive(Debug, Clone, Copy, PartialEq, Eq)]
158pub enum VelocityFrame {
159    /// NED (North-East-Down) local frame.
160    LocalFrameNed = 0,
161    /// FRD (Forward-Right-Down) local frame.
162    LocalFrameFrd = 1,
163    /// Body FRD frame.
164    BodyFrameFrd = 2,
165}
166
167impl EkfImuSample {
168    /// Construct an IMU sample from delta-angle and delta-velocity values.
169    #[inline]
170    pub const fn new(
171        time_us: u64,
172        delta_ang: [f32; 3],
173        delta_vel: [f32; 3],
174        delta_ang_dt: f32,
175        delta_vel_dt: f32,
176    ) -> Self {
177        Self {
178            time_us,
179            delta_ang,
180            delta_vel,
181            delta_ang_dt,
182            delta_vel_dt,
183            delta_vel_clipping: [0; 3],
184            _pad: [0; 5],
185        }
186    }
187
188    /// Construct an IMU sample with explicit clipping flags.
189    #[inline]
190    pub const fn with_clipping(
191        time_us: u64,
192        delta_ang: [f32; 3],
193        delta_vel: [f32; 3],
194        delta_ang_dt: f32,
195        delta_vel_dt: f32,
196        delta_vel_clipping: [bool; 3],
197    ) -> Self {
198        Self {
199            time_us,
200            delta_ang,
201            delta_vel,
202            delta_ang_dt,
203            delta_vel_dt,
204            delta_vel_clipping: [
205                delta_vel_clipping[0] as u8,
206                delta_vel_clipping[1] as u8,
207                delta_vel_clipping[2] as u8,
208            ],
209            _pad: [0; 5],
210        }
211    }
212}
213
214#[cfg(feature = "gnss")]
215impl EkfGnssSample {
216    /// Construct a GNSS sample.
217    ///
218    /// `pdop` is the position dilution of precision reported by the GNSS
219    /// receiver and is compared against the EKF GNSS PDOP threshold.
220    #[inline]
221    #[allow(clippy::too_many_arguments)]
222    pub fn new(
223        time_us: u64,
224        lat: f64,
225        lon: f64,
226        alt: f32,
227        vel: [f32; 3],
228        hacc: f32,
229        vacc: f32,
230        sacc: f32,
231        pdop: f32,
232        fix_type: u8,
233        nsats: u8,
234    ) -> Self {
235        Self {
236            time_us,
237            lat,
238            lon,
239            alt,
240            yaw: f32::NAN,
241            yaw_acc: f32::NAN,
242            yaw_offset: 0.0,
243            vel,
244            hacc,
245            vacc,
246            sacc,
247            pdop,
248            fix_type,
249            nsats,
250            spoofed: 0,
251            jammed: 0,
252            _pad: [0; 8],
253        }
254    }
255}
256
257#[cfg(feature = "magnetometer")]
258impl EkfMagSample {
259    /// Construct a magnetometer sample.
260    #[inline]
261    pub const fn new(time_us: u64, mag: [f32; 3]) -> Self {
262        Self {
263            time_us,
264            mag,
265            reset: 0,
266            _pad: [0; 3],
267        }
268    }
269
270    /// Mark this sample as a sensor-replacement or calibration-change event.
271    ///
272    /// Set when the magnetometer source changed mid-flight so the EKF can
273    /// reinitialise its magnetometer states.
274    #[inline]
275    pub const fn with_reset(mut self) -> Self {
276        self.reset = 1;
277        self
278    }
279}
280
281#[cfg(feature = "barometer")]
282impl EkfBaroSample {
283    /// Construct a barometer sample.
284    #[inline]
285    pub const fn new(time_us: u64, hgt: f32) -> Self {
286        Self {
287            time_us,
288            hgt,
289            reset: 0,
290            _pad: [0; 3],
291        }
292    }
293
294    /// Mark this sample as a sensor-replacement or calibration-change event.
295    ///
296    /// Set when the barometer source changed mid-flight so the EKF can
297    /// reinitialise its barometer states.
298    #[inline]
299    pub const fn with_reset(mut self) -> Self {
300        self.reset = 1;
301        self
302    }
303}
304
305#[cfg(feature = "airspeed")]
306impl EkfAirspeedSample {
307    /// Construct an airspeed sample.
308    #[inline]
309    pub const fn new(time_us: u64, true_airspeed: f32, eas2tas: f32) -> Self {
310        Self {
311            time_us,
312            true_airspeed,
313            eas2tas,
314        }
315    }
316}
317
318#[cfg(feature = "range-finder")]
319impl EkfRangeSample {
320    /// Construct a range-finder sample.
321    ///
322    /// `quality` is 0–100 (100 = perfect signal, 0 = invalid, -1 = unknown).
323    #[inline]
324    pub const fn new(time_us: u64, rng: f32, quality: i8) -> Self {
325        Self {
326            time_us,
327            rng,
328            quality,
329            _pad: [0; 3],
330        }
331    }
332}
333
334#[cfg(feature = "optical-flow")]
335impl EkfFlowSample {
336    /// Construct an optical-flow sample.
337    ///
338    /// `flow_rate_xy` and `gyro_rate_xyz` must be rates in rad/s (not integrated values).
339    #[inline]
340    pub const fn new(
341        time_us: u64,
342        flow_rate_xy: [f32; 2],
343        gyro_rate_xyz: [f32; 3],
344        quality: u8,
345    ) -> Self {
346        Self {
347            time_us,
348            flow_rate_xy,
349            gyro_rate_xyz,
350            quality,
351            _pad: [0; 3],
352        }
353    }
354}
355
356#[cfg(feature = "external-vision")]
357impl EkfExtVisionSample {
358    /// Construct an external-vision sample.
359    ///
360    /// Defaults: `pos_frame = LocalFrameFrd`, `vel_frame = BodyFrameFrd`,
361    /// `reset_counter = 0`, `quality = 0`.
362    ///
363    /// `ang_var` is per-axis attitude variance in rad^2 for X, Y, Z.
364    /// Use `[v; 3]` to broadcast a single scalar variance to all axes.
365    #[inline]
366    #[allow(clippy::too_many_arguments)]
367    pub const fn new(
368        time_us: u64,
369        pos: [f32; 3],
370        quat: [f32; 4],
371        vel: [f32; 3],
372        pos_var: [f32; 3],
373        ang_var: [f32; 3],
374        vel_var: [f32; 3],
375    ) -> Self {
376        Self {
377            time_us,
378            pos,
379            quat,
380            vel,
381            pos_var,
382            ang_var,
383            vel_var,
384            pos_frame: PositionFrame::LocalFrameFrd as u8,
385            vel_frame: VelocityFrame::BodyFrameFrd as u8,
386            reset_counter: 0,
387            quality: 0,
388        }
389    }
390
391    /// Override the position and velocity reference frames.
392    #[inline]
393    pub const fn with_frames(mut self, pos_frame: PositionFrame, vel_frame: VelocityFrame) -> Self {
394        self.pos_frame = pos_frame as u8;
395        self.vel_frame = vel_frame as u8;
396        self
397    }
398
399    /// Override the quality indicator (0–100; 100 = perfect, 0 = invalid).
400    #[inline]
401    pub const fn with_quality(mut self, quality: i8) -> Self {
402        self.quality = quality;
403        self
404    }
405
406    /// Increment the reset counter to signal a tracker discontinuity.
407    ///
408    /// The EKF uses this to detect when the external-vision pose estimate
409    /// has jumped (e.g. loop-closure, tracking re-initialisation).
410    #[inline]
411    pub const fn with_reset_counter(mut self, reset_counter: u8) -> Self {
412        self.reset_counter = reset_counter;
413        self
414    }
415}
416
417#[cfg(feature = "aux-vel")]
418impl EkfAuxVelSample {
419    /// Construct an auxiliary velocity sample.
420    #[inline]
421    pub const fn new(time_us: u64, vel: [f32; 2], vel_var: [f32; 2]) -> Self {
422        Self {
423            time_us,
424            vel,
425            vel_var,
426        }
427    }
428}
429
430impl EkfSystemFlagUpdate {
431    /// Construct a system-flag update sample.
432    ///
433    /// Defaults:
434    /// - `in_air = true`
435    /// - all other flags = `false`
436    #[inline]
437    pub const fn new(time_us: u64) -> Self {
438        Self {
439            time_us,
440            at_rest: false,
441            in_air: true,
442            is_fixed_wing: false,
443            gnd_effect: false,
444            constant_pos: false,
445            in_transition_to_fw: false,
446            _pad: 0,
447        }
448    }
449
450    /// Set all mode flags explicitly.
451    #[inline]
452    #[allow(clippy::too_many_arguments)]
453    pub const fn with_flags(
454        mut self,
455        at_rest: bool,
456        in_air: bool,
457        is_fixed_wing: bool,
458        gnd_effect: bool,
459        constant_pos: bool,
460        in_transition_to_fw: bool,
461    ) -> Self {
462        self.at_rest = at_rest;
463        self.in_air = in_air;
464        self.is_fixed_wing = is_fixed_wing;
465        self.gnd_effect = gnd_effect;
466        self.constant_pos = constant_pos;
467        self.in_transition_to_fw = in_transition_to_fw;
468        self
469    }
470}