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}