calibsession/
lib.rs

1//! The live tag calibration, and the compass angles every CompassTag display
2//! shows, through the C++ core.
3//!
4//! [`Session`] is a safe wrapper over the C interface in
5//! `host/libraries/tagcore-c/calibsession_c.h`, which wraps sensoranalysis's
6//! Qt-free `calibcore::CalibrationSession` -- the same code qtcalibrate's
7//! `CompassData` now runs. So a Rust program calibrates exactly as qtcalibrate
8//! does: Freescale's solver, eviction by leverage and outliers, the
9//! accelerometer's own fit, the quality figures and the filtered live compass.
10//! [`display_angles`] and [`model_rotation`] are the conversions qtcalibrate's
11//! displays apply to an orientation.
12//!
13//! ```no_run
14//! use calibsession::Session;
15//! # let samples: Vec<([f32; 3], Option<[f32; 3]>)> = vec![];
16//! let mut session = Session::new()?;
17//! for (i, (mag, accel)) in samples.iter().enumerate() {
18//!     if session.add_sample(*mag, *accel) {
19//!         // the solver accepted a new fit
20//!     }
21//!     if i % 2 == 1 {
22//!         session.update_quality(); // the caller's tick
23//!     }
24//! }
25//! let quality = session.quality();
26//! # Ok::<(), calibsession::SessionError>(())
27//! ```
28//!
29//! Units are the calibration stream's: magnetometer in µT, accelerometer in
30//! mg. Quaternions are `[w, x, y, z]`.
31//!
32//! # Linking
33//!
34//! As for `taglogwriter`: `tagcore-c-sys` links `libtagcore_c`, found through
35//! `TAGCORE_C_LIB_DIR`, statically on macOS and shared on Linux and Windows.
36//! It is the one crate that declares `links = "tagcore_c"`, so a program may
37//! use this crate beside the other wrappers over the library.
38//!
39//! # Safety model
40//!
41//! All `unsafe` is in this file. A [`Session`] owns exactly one C handle,
42//! never null, freed exactly once in `Drop`. Every pointer passed in points
43//! at Rust memory that lives until the call returns, sized as the header
44//! requires; the library keeps no pointer after a call. Output structs are
45//! `#[repr(C)]` mirrors of the header's, zero-initialized before the call
46//! fills them. A session may move between threads but not be shared:
47//! `Session` is `Send` and not `Sync`, and every call that advances it takes
48//! `&mut self`.
49//!
50//! # Maintaining
51//!
52//! The declarations, imported here as `ffi`, are `tagcore_c_sys::tcs`
53//! (`host/libraries/tagcore-c-sys/src/tcs.rs`), written by hand from
54//! `calibsession_c.h`. When the
55//! header changes, change the declaration and the safe method over it; if a
56//! signature, meaning or struct layout changed, the header raises
57//! `TCS_ABI_VERSION`, and `ffi::TCS_ABI_VERSION` must follow.
58//! [`Session::new`] refuses a library built from another version.
59
60use std::fmt;
61use std::ptr::NonNull;
62
63/// The raw C interface, in `tagcore-c-sys`, which links the library.
64use tagcore_c_sys::tcs as ffi;
65
66
67/// Buffer slots in a session: [`Session::points`] never returns more.
68pub const SLOTS: usize = 650;
69
70/// Why a session could not be made.
71#[derive(Debug, Clone, PartialEq, Eq)]
72pub enum SessionError {
73    /// The library was built from another version of `calibsession_c.h`.
74    AbiMismatch {
75        /// The version this crate expects.
76        expected: u32,
77        /// The version the library reports.
78        found: u32,
79    },
80    /// The library could not allocate a session.
81    OutOfMemory,
82}
83
84impl fmt::Display for SessionError {
85    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
86        match self {
87            SessionError::AbiMismatch { expected, found } => write!(
88                f,
89                "libtagcore_c's calibration interface is version {found}, this program needs {expected}; \
90                 rebuild them together"
91            ),
92            SessionError::OutOfMemory => write!(f, "out of memory creating a calibration session"),
93        }
94    }
95}
96
97impl std::error::Error for SessionError {}
98
99/// The magnetometer calibration constants.
100#[derive(Debug, Clone, Copy, PartialEq)]
101pub struct Constants {
102    /// Whether a calibration exists (fitted or loaded). The values are the
103    /// identity and 50 µT when it does not.
104    pub valid: bool,
105    /// Field magnitude B, µT.
106    pub field: f32,
107    /// Hard-iron offset V, µT.
108    pub offset: [f32; 3],
109    /// Inverse soft-iron matrix A, row-major.
110    pub mapping: [[f32; 3]; 3],
111}
112
113/// MagQuality's figures; see `magquality_core.h` for what each means.
114#[derive(Debug, Clone, Copy, PartialEq, Default)]
115pub struct Quality {
116    /// Occupied magnetometer patches.
117    pub patches_seen: i32,
118    /// Patches available.
119    pub mag_patches: i32,
120    /// patches_seen / mag_patches.
121    pub coverage: f32,
122    /// Samples in the emptiest occupied patch.
123    pub patch_min: i32,
124    /// Samples in the fullest patch.
125    pub patch_max: i32,
126    /// Direction evenness, 0 to 1.
127    pub isotropy: f32,
128    /// Gravity patches per occupied magnetometer patch.
129    pub attitude_diversity: f32,
130    /// Distinct (magnetometer, gravity) cells.
131    pub attitude_cells: i32,
132    /// Magnetometer patches with a gravity observation.
133    pub attitude_patches: i32,
134    /// Median calibrated magnitude, µT.
135    pub field: f32,
136    /// RMS residual, percent of the field.
137    pub fit_error: f32,
138    /// Robust residual spread, percent of the field.
139    pub residual_spread: f32,
140    /// 95th percentile residual, percent of the field.
141    pub residual_p95: f32,
142    /// Hard iron the fit left, µT.
143    pub residual_hard_iron: f32,
144    /// Median inclination, degrees.
145    pub dip_mean_deg: f32,
146    /// Robust inclination spread, degrees.
147    pub dip_spread_deg: f32,
148    /// 95th percentile inclination deviation, degrees.
149    pub dip_p95_deg: f32,
150    /// Samples that passed the accelerometer gate.
151    pub dip_samples: i32,
152    /// Samples offered with an accelerometer.
153    pub accel_samples: i32,
154    /// Whether the dip figures exist.
155    pub have_accel_metrics: bool,
156    /// Samples accepted.
157    pub samples: i32,
158}
159
160/// The accelerometer offset in force: the live fit, or one loaded from a tag.
161#[derive(Debug, Clone, Copy, PartialEq, Default)]
162pub struct AccelFit {
163    /// Whether offset, radius and residual are a fit or a loaded offset.
164    pub valid: bool,
165    /// Zero-g offset, mg.
166    pub offset: [f32; 3],
167    /// Fitted sphere radius, mg; 0 when loaded.
168    pub radius: f32,
169    /// RMS distance from the sphere, mg.
170    pub residual: f32,
171    /// Readings the fit was built from.
172    pub samples: i32,
173    /// Occupied direction patches.
174    pub patches_seen: i32,
175    /// Patches in the lattice.
176    pub patches: i32,
177    /// patches_seen / patches.
178    pub coverage: f32,
179    /// Direction evenness, 0 to 1.
180    pub isotropy: f32,
181    /// Readings offered since the last clear.
182    pub offered: i32,
183    /// Of those, rejected as motion.
184    pub gated: i32,
185}
186
187/// Evictions from the full magnetometer buffer since the last clear.
188#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
189pub struct Evictions {
190    /// All evictions, including the random fallback's.
191    pub total: i32,
192    /// Decided as the least informative sample.
193    pub leverage: i32,
194    /// Decided as an outlier.
195    pub outlier: i32,
196}
197
198/// One solved compass orientation.
199#[derive(Debug, Clone, Copy, PartialEq, Default)]
200pub struct Orientation {
201    /// Magnetic-frame orientation, `[w, x, y, z]`.
202    pub q: [f32; 4],
203    /// Dip angle, degrees.
204    pub dip: f32,
205    /// Field magnitude after calibration, µT.
206    pub field: f32,
207    /// Acceleration magnitude, mg.
208    pub mg: f32,
209}
210
211/// Display angles of an orientation, before declination and battery direction.
212#[derive(Debug, Clone, Copy, PartialEq, Default)]
213pub struct Angles {
214    /// Magnetic heading, degrees clockwise from north, [0, 360).
215    pub yaw: f64,
216    /// About the tag x axis, degrees.
217    pub pitch: f64,
218    /// About the tag y axis, degrees.
219    pub roll: f64,
220}
221
222/// One live calibration: the sample buffer, the solver, retention, the
223/// accelerometer fit, the quality figures and the filtered compass.
224pub struct Session {
225    /// The C handle; never null, freed in `Drop`.
226    ptr: NonNull<ffi::TcsSession>,
227}
228
229// SAFETY: the handle owns its memory and has no thread affinity; the C
230// interface only forbids concurrent use, which `!Sync` and `&mut self`
231// already prevent.
232unsafe impl Send for Session {}
233
234impl Session {
235    /// Creates an empty session.
236    ///
237    /// # Errors
238    ///
239    /// [`SessionError::AbiMismatch`] when the linked library was built from
240    /// another version of the header; [`SessionError::OutOfMemory`] when it
241    /// cannot allocate.
242    pub fn new() -> Result<Session, SessionError> {
243        // SAFETY: takes nothing, returns a constant.
244        let found = unsafe { ffi::tcs_abi_version() };
245        if found != ffi::TCS_ABI_VERSION {
246            return Err(SessionError::AbiMismatch {
247                expected: ffi::TCS_ABI_VERSION,
248                found,
249            });
250        }
251        // SAFETY: takes nothing; returns an owned handle or null.
252        let ptr = NonNull::new(unsafe { ffi::tcs_new() }).ok_or(SessionError::OutOfMemory)?;
253        Ok(Session { ptr })
254    }
255
256    /// Discards every sample, the fit, the accelerometer fit and any loaded
257    /// offset; the field returns to 50 µT. The live compass filter is kept.
258    pub fn clear(&mut self) {
259        // SAFETY: a live handle, used from one thread.
260        unsafe { ffi::tcs_clear(self.ptr.as_ptr()) }
261    }
262
263    /// Feeds one calibration sample: raw magnetometer, µT, and the
264    /// accelerometer reading, mg, when the sample has one.
265    ///
266    /// Returns true when the solver accepted a new fit on this sample.
267    pub fn add_sample(&mut self, mag: [f32; 3], accel: Option<[f32; 3]>) -> bool {
268        let a = accel.unwrap_or_default();
269        // SAFETY: a live handle; both arrays hold three floats and outlive
270        // the call.
271        unsafe {
272            ffi::tcs_add_sample(
273                self.ptr.as_ptr(),
274                mag.as_ptr(),
275                accel.is_some().into(),
276                a.as_ptr(),
277            ) != 0
278        }
279    }
280
281    /// Whether a magnetometer calibration exists (fitted or loaded).
282    pub fn has_calibration(&self) -> bool {
283        // SAFETY: a live handle.
284        unsafe { ffi::tcs_has_calibration(self.ptr.as_ptr()) != 0 }
285    }
286
287    /// Applies the current magnetometer calibration, `A (mag - V)`; the
288    /// identity before a fit.
289    pub fn calibrate(&self, mag: [f32; 3]) -> [f32; 3] {
290        let mut out = [0.0f32; 3];
291        // SAFETY: a live handle; three floats in, room for three out.
292        unsafe { ffi::tcs_calibrate(self.ptr.as_ptr(), mag.as_ptr(), out.as_mut_ptr()) };
293        out
294    }
295
296    /// Every buffered reading, calibrated, in slot order; at most [`SLOTS`].
297    pub fn points(&self) -> Vec<[f32; 3]> {
298        let mut xyz = vec![0.0f32; 3 * SLOTS];
299        // SAFETY: a live handle; `xyz` has room for SLOTS points, and the
300        // library writes at most `max` of them.
301        let n = unsafe { ffi::tcs_points(self.ptr.as_ptr(), xyz.as_mut_ptr(), SLOTS) }.min(SLOTS);
302        xyz.as_chunks::<3>().0.iter().take(n).copied().collect()
303    }
304
305    /// The current magnetometer constants.
306    pub fn constants(&self) -> Constants {
307        let mut field = 0.0f32;
308        let mut offset = [0.0f32; 3];
309        let mut a = [0.0f32; 9];
310        // SAFETY: a live handle; one float, three and nine of room.
311        let valid = unsafe {
312            ffi::tcs_constants(
313                self.ptr.as_ptr(),
314                &mut field,
315                offset.as_mut_ptr(),
316                a.as_mut_ptr(),
317            )
318        } != 0;
319        Constants {
320            valid,
321            field,
322            offset,
323            mapping: [[a[0], a[1], a[2]], [a[3], a[4], a[5]], [a[6], a[7], a[8]]],
324        }
325    }
326
327    /// Installs magnetometer constants read back from a tag. The field is
328    /// recomputed from the buffered readings, or kept when there are none.
329    pub fn set_constants(&mut self, offset: [f32; 3], mapping: [[f32; 3]; 3]) {
330        let a: Vec<f32> = mapping.iter().flatten().copied().collect();
331        // SAFETY: a live handle; three and nine floats that outlive the call.
332        unsafe { ffi::tcs_set_constants(self.ptr.as_ptr(), offset.as_ptr(), a.as_ptr()) }
333    }
334
335    /// The field magnitude B, µT.
336    pub fn field(&self) -> f32 {
337        // SAFETY: a live handle.
338        unsafe { ffi::tcs_field(self.ptr.as_ptr()) }
339    }
340
341    /// Installs an accelerometer offset read back from a tag, mg. It stands
342    /// until a live fit is valid or the session is cleared.
343    pub fn set_accel_offset(&mut self, offset: [f32; 3]) {
344        // SAFETY: a live handle; three floats.
345        unsafe { ffi::tcs_set_accel_offset(self.ptr.as_ptr(), offset.as_ptr()) }
346    }
347
348    /// Recomputes the quality figures and the accelerometer fit. A valid fit
349    /// also tightens the accelerometer's intake gate, so how often this runs
350    /// affects later samples: qtcalibrate's replay runs it every second
351    /// sample.
352    pub fn update_quality(&mut self) {
353        // SAFETY: a live handle.
354        unsafe { ffi::tcs_update_quality(self.ptr.as_ptr()) }
355    }
356
357    /// The quality figures as of the last [`Session::update_quality`].
358    pub fn quality(&self) -> Quality {
359        let mut q = ffi::TcsQuality::default();
360        // SAFETY: a live handle; `q` is a repr(C) mirror of tcs_quality.
361        unsafe { ffi::tcs_get_quality(self.ptr.as_ptr(), &mut q) };
362        Quality {
363            patches_seen: q.patches_seen,
364            mag_patches: q.mag_patches,
365            coverage: q.coverage,
366            patch_min: q.patch_min,
367            patch_max: q.patch_max,
368            isotropy: q.isotropy,
369            attitude_diversity: q.attitude_diversity,
370            attitude_cells: q.attitude_cells,
371            attitude_patches: q.attitude_patches,
372            field: q.field,
373            fit_error: q.fit_error,
374            residual_spread: q.residual_spread,
375            residual_p95: q.residual_p95,
376            residual_hard_iron: q.residual_hard_iron,
377            dip_mean_deg: q.dip_mean_deg,
378            dip_spread_deg: q.dip_spread_deg,
379            dip_p95_deg: q.dip_p95_deg,
380            dip_samples: q.dip_samples,
381            accel_samples: q.accel_samples,
382            have_accel_metrics: q.have_accel_metrics != 0,
383            samples: q.samples,
384        }
385    }
386
387    /// The accelerometer offset in force.
388    pub fn accel_fit(&self) -> AccelFit {
389        let mut a = ffi::TcsAccelFit::default();
390        // SAFETY: a live handle; `a` is a repr(C) mirror of tcs_accel_fit.
391        unsafe { ffi::tcs_get_accel_fit(self.ptr.as_ptr(), &mut a) };
392        AccelFit {
393            valid: a.valid != 0,
394            offset: a.offset,
395            radius: a.radius,
396            residual: a.residual,
397            samples: a.samples,
398            patches_seen: a.patches_seen,
399            patches: a.patches,
400            coverage: a.coverage,
401            isotropy: a.isotropy,
402            offered: a.offered,
403            gated: a.gated,
404        }
405    }
406
407    /// Evictions since the last clear.
408    pub fn evictions(&self) -> Evictions {
409        let mut out = [0; 3];
410        // SAFETY: a live handle; room for three ints.
411        unsafe { ffi::tcs_evictions(self.ptr.as_ptr(), out.as_mut_ptr()) };
412        Evictions {
413            total: out[0],
414            leverage: out[1],
415            outlier: out[2],
416        }
417    }
418
419    /// Solves the live compass from one raw sample, through the session's
420    /// low-pass filter, which this advances.
421    ///
422    /// Returns `None` when the filtered pair admits no orientation.
423    pub fn orientation(&mut self, mag: [f32; 3], accel: [f32; 3]) -> Option<Orientation> {
424        let mut o = ffi::TcsOrientation::default();
425        // SAFETY: a live handle; three floats each in; `o` is a repr(C)
426        // mirror of tcs_orientation.
427        let ok = unsafe {
428            ffi::tcs_orientation_of(self.ptr.as_ptr(), mag.as_ptr(), accel.as_ptr(), &mut o)
429        } != 0;
430        ok.then_some(Orientation {
431            q: o.q,
432            dip: o.dip,
433            field: o.field,
434            mg: o.mg,
435        })
436    }
437}
438
439impl Drop for Session {
440    fn drop(&mut self) {
441        // SAFETY: the handle is live and owned by this value alone; it is
442        // freed exactly once, here.
443        unsafe { ffi::tcs_free(self.ptr.as_ptr()) }
444    }
445}
446
447/// Display angles of an orientation quaternion `[w, x, y, z]`: the heading,
448/// pitch and roll every CompassTag display shows, before declination and
449/// battery direction.
450pub fn display_angles(q: [f32; 4]) -> Angles {
451    let mut a = ffi::TcsAngles::default();
452    // SAFETY: four floats in; `a` is a repr(C) mirror of tcs_angles.
453    unsafe { ffi::tcs_display_angles(q.as_ptr(), &mut a) };
454    Angles {
455        yaw: a.yaw,
456        pitch: a.pitch,
457        roll: a.roll,
458    }
459}
460
461/// The rotation qtcalibrate applies to its tag model, `Rz(yaw) * Ry(roll) *
462/// Rx(pitch)`, degrees, yaw counter-clockwise; `[w, x, y, z]`.
463pub fn model_rotation(pitch: f32, roll: f32, yaw: f32) -> [f32; 4] {
464    let mut q = [0.0f32; 4];
465    // SAFETY: room for four floats.
466    unsafe { ffi::tcs_model_rotation(pitch, roll, yaw, q.as_mut_ptr()) };
467    q
468}
calibsession/lib.rs

calibsession/
lib.rs
