Merges up to 4 IMUDataSource instances by capability and fuses their samples into an attitude estimate via a pluggable AhrsFilterBase. More...

#include <ahrs_controller.hpp>

Inheritance diagram for m5::imu::AhrsController:

Classes
struct	calibration_data_t
	Exportable/importable snapshot of every capability's auto-calibration offset. More...

Public Member Functions
	AhrsController ()
	Construct with no sources and the default filter (Complementary)

	~AhrsController ()
	Destructor.

	AhrsController (const AhrsController &)=delete
	Copy construction is disabled (holds pointers to registered sources)

AhrsController &	operator= (const AhrsController &)=delete
	Copy assignment is disabled (holds pointers to registered sources)

bool	add (IMUDataSource &src)
	Register an IMU data source.

bool	writeAxisOrder (const IMUDataSource &src, const AxisOrder &order)
	Set the chip-frame-to-body-frame axis remap applied to one source's Accel/Gyro (and Temp) samples.

bool	readAxisOrder (const IMUDataSource &src, AxisOrder &out) const
	Read back the Accel/Gyro axis remap currently configured for one source.

bool	writeMagAxisOrder (const IMUDataSource &src, const AxisOrder &order)
	Set the chip-frame-to-body-frame axis remap applied to one source's Mag samples.

bool	readMagAxisOrder (const IMUDataSource &src, AxisOrder &out) const
	Read back the Mag-only axis remap currently configured for one source.

void	writeUserAxisOrder (const AxisOrder &order)
	Set a global user axis remap composed on top of every source's fixed order.

void	readUserAxisOrder (AxisOrder &out) const
	Read back the current global user axis remap.

void	writeAutoCalibration (const uint8_t accel, const uint8_t gyro, const uint8_t mag)
	Enable/disable continuous background auto-calibration per capability.

void	readAutoCalibration (uint8_t &accel, uint8_t &gyro, uint8_t &mag) const
	Read back the current auto-calibration strengths.

void	writeMagCalibrationRadius (const float radius_ut)
	Pin the field strength the magnetometer auto-calibration fits its samples to.

float	readMagCalibrationRadius () const
	Field strength the magnetometer auto-calibration is currently fitting to.

bool	calibrateGyro (const uint32_t duration_ms)
	Collect gyro samples for the given duration (real clock) and set the gyro offset to their mean.

bool	calibrateGyro (const uint32_t update_count, const float dt)
	Deterministic sibling of calibrateGyro(const uint32_t): explicit dt instead of the real clock.

bool	readCalibration (calibration_data_t &out) const
	Snapshot every capability's current auto-calibration offset.

bool	writeCalibration (const calibration_data_t &data)
	Overwrite every capability's auto-calibration offset from a snapshot.

bool	begin ()
	Prepare this controller for update()

Capability	update ()
	Drain every source and fuse newly-arrived samples, measuring dt from the real clock (m5::utility::micros())

Capability	update (const float dt)
	Drain every source and fuse newly-arrived samples using an explicit dt.

bool	sleep ()
	Put every added source into its lowest reversible power state.

bool	wakeup ()
	Bring every added source back from sleep()

bool	selectFilter (const FilterType type)
	Select and internally construct one of the built-in filters.

void	setFilter (AhrsFilterBase &filter)
	Inject an externally-owned filter.

AhrsFilterBase *	filter () const
	Currently active filter, or nullptr if none has been selected yet.

void	quaternion (float &w, float &x, float &y, float &z) const
	Current fused attitude.

void	euler (float &roll_deg, float &pitch_deg, float &yaw_deg) const
	Current fused attitude as aerospace ZYX Euler angles.

void	tareYaw ()
	Zero the publicly reported yaw at the current heading.

void	clearYawTare ()
	Remove a previously applied tareYaw() offset.

const imu_physical_data_t &	latest () const
	Most recently fused physical-unit sample.

bool	hasAccel () const
	Whether an accelerometer source has been registered (see add())

bool	hasGyro () const
	Whether a gyroscope source has been registered (see add())

bool	hasTemp () const
	Whether a temperature source has been registered (see add())

bool	hasValidMag () const
	Whether at least one valid magnetometer sample has been seen.

unsigned long	lastMicros () const
	Microsecond timestamp of the most recent self-clocked update()

Detailed Description

Merges up to 4 IMUDataSource instances by capability and fuses their samples into an attitude estimate via a pluggable AhrsFilterBase.

Capability merge: add() assigns each of Accel/Gyro/Mag/Temp to the first source that reports it ("first come, first served"); later sources reporting an already-claimed capability are still drained every update() (so their internal ring buffers do not overflow) but their samples for that capability are discarded.
Primary clock: the gyro-owning source drives the filter. Every update() call drains the gyro source fully; each popped gyro sample triggers one AhrsFilterBase::update() call, with the elapsed time divided evenly across the batch (dt / <samples drained this call>). The time of a call that drains no gyro sample is carried over to the next call that does, so calling update() faster than the gyro ODR does not shrink the integrated rotation. The carried-over time is capped at 0.5 s, so a stalled gyro source does not hand the filter one huge dt when it resumes. Accel/mag samples from a different source than the gyro are drained once per update() call and the resulting "latest" value is reused for every filter update() call in that batch (no per-sample interpolation across sources); a source shared with the gyro (e.g. a single 6/9-axis chip) is synchronized sample-for-sample instead.
Magnetometer overflow: a mag sample where any axis reads exactly -32768 (the BMM150 OVERFLOW sentinel) is rejected outright; the previous valid mag value is kept (both for latest() and for whatever gets passed to the filter). If no valid mag sample has ever been seen, the filter is fed mag == nullptr (6-axis operation).
Clocking: update() measures dt itself via m5::utility::micros() and smooths it with an exponential moving average (see ahrs_controller.cpp) before handing it to the filter, to avoid feeding raw, jittery sample-to-sample deltas straight into filters that resample their internal state on large dt swings (see vqf.hpp). update(const float dt) bypasses both the clock and the smoothing – the caller's dt is used as-is for the whole batch drained during that call, which is what callers with their own timebase should use.

Member Function Documentation

◆ add()

bool m5::imu::AhrsController::add ( IMUDataSource & src )

Register an IMU data source.

Parameters

src	Source to add (must outlive this controller)

Returns: False if this controller already holds the maximum number of sources

Never fails due to a capability already being claimed by another source – duplicate capabilities are simply ignored (the first source to report them wins).

◆ begin()

bool m5::imu::AhrsController::begin ( )

Prepare this controller for update()

Returns: False if no source has claimed both Accel and Gyro

Selects FilterType::Madgwick if no filter has been chosen yet (see selectFilter()/setFilter()).

◆ calibrateGyro() [1/2]

bool m5::imu::AhrsController::calibrateGyro ( const uint32_t duration_ms )

Collect gyro samples for the given duration (real clock) and set the gyro offset to their mean.

Parameters

duration_ms Collection duration, milliseconds

Returns: False if no source has claimed Gyro, or no sample was drained during that time

Intended for hardware use: repeatedly calls update() (real clock, via m5::utility::millis()) for approximately duration_ms while the unit is held still, then overwrites the gyro offset with the mean of every gyro sample seen. See calibrateGyro(const uint32_t, const float) for the deterministic, explicit-dt sibling (drives the same sample collection through update(const float dt) instead of the real clock).

◆ calibrateGyro() [2/2]

bool m5::imu::AhrsController::calibrateGyro	(	const uint32_t	update_count,
		const float	dt )

Deterministic sibling of calibrateGyro(const uint32_t): explicit dt instead of the real clock.

Parameters

update_count	Number of update(dt) calls to run while collecting
dt	Time step passed to each update(dt) call, seconds

Returns: False if no source has claimed Gyro, or no sample was drained during that time

◆ clearYawTare()

void m5::imu::AhrsController::clearYawTare ( )

Remove a previously applied tareYaw() offset.

Resets the internal yaw tare to identity, so quaternion()/euler() once again report the filter's raw heading as-is (e.g. relative to magnetic north on a 9-axis setup).

◆ euler()

void m5::imu::AhrsController::euler	(	float &	roll_deg,
		float &	pitch_deg,
		float &	yaw_deg ) const

Current fused attitude as aerospace ZYX Euler angles.

Parameters

[out]	roll_deg	Roll (X axis), degrees
[out]	pitch_deg	Pitch (Y axis), degrees
[out]	yaw_deg	Yaw (Z axis), degrees

◆ hasValidMag()

bool m5::imu::AhrsController::hasValidMag ( ) const

inline

Whether at least one valid magnetometer sample has been seen.

False on a 6-axis setup, or before the first non-overflow mag sample arrives (see the "Magnetometer overflow" note on this class). latest().mag stays at its last valid value (or zero) while this reads false.

◆ lastMicros()

unsigned long m5::imu::AhrsController::lastMicros ( ) const

inline

Microsecond timestamp of the most recent self-clocked update()

Stamped only by the update() overload that measures dt itself; update(const float dt) leaves it untouched, so this stays 0 for callers driving the controller entirely from their own timebase.

◆ quaternion()

void m5::imu::AhrsController::quaternion	(	float &	w,
		float &	x,
		float &	y,
		float &	z ) const

Current fused attitude.

Parameters

[out]	w	Quaternion scalar part
[out]	x	Quaternion vector part X
[out]	y	Quaternion vector part Y
[out]	z	Quaternion vector part Z

Yields the identity quaternion if no filter has been selected/injected yet.

◆ readAutoCalibration()

void m5::imu::AhrsController::readAutoCalibration	(	uint8_t &	accel,
		uint8_t &	gyro,
		uint8_t &	mag ) const

Read back the current auto-calibration strengths.

Parameters

[out]	accel	Accelerometer strength
[out]	gyro	Gyroscope strength
[out]	mag	Magnetometer strength

◆ readAxisOrder()

bool m5::imu::AhrsController::readAxisOrder	(	const IMUDataSource &	src,
		AxisOrder &	out ) const

Read back the Accel/Gyro axis remap currently configured for one source.

Parameters

	src	Source to query (must already have been add()ed)
[out]	out	Destination for the current AxisOrder

Returns: False (out left untouched) if src was never add()ed to this controller

◆ readCalibration()

bool m5::imu::AhrsController::readCalibration ( calibration_data_t & out ) const

Snapshot every capability's current auto-calibration offset.

Parameters

[out] out Destination for the snapshot

Returns: Always true

◆ readMagAxisOrder()

bool m5::imu::AhrsController::readMagAxisOrder	(	const IMUDataSource &	src,
		AxisOrder &	out ) const

Read back the Mag-only axis remap currently configured for one source.

Parameters

	src	Source to query (must already have been add()ed)
[out]	out	Destination for the current AxisOrder

Returns: False (out left untouched) if src was never add()ed to this controller

◆ readMagCalibrationRadius()

float m5::imu::AhrsController::readMagCalibrationRadius ( ) const

Field strength the magnetometer auto-calibration is currently fitting to.

Returns: Radius in uT; 0 while the estimate is still unseeded (no calibration has run yet)

◆ readUserAxisOrder()

void m5::imu::AhrsController::readUserAxisOrder ( AxisOrder & out ) const

Read back the current global user axis remap.

Parameters

[out] out Destination for the current user AxisOrder (identity by default)

◆ selectFilter()

bool m5::imu::AhrsController::selectFilter ( const FilterType type )

Select and internally construct one of the built-in filters.

Parameters

type	Filter to construct

Returns: True on success

Replaces (and resets) whatever filter is currently active, deleting a previously internally-constructed filter first. Does not delete a filter injected via setFilter().

◆ setFilter()

void m5::imu::AhrsController::setFilter ( AhrsFilterBase & filter )

Inject an externally-owned filter.

Parameters

filter Filter instance to use from now on (must outlive this controller)

Deletes a previously internally-constructed filter (selectFilter()), if any. Ownership of filter itself is not taken.

◆ sleep()

bool m5::imu::AhrsController::sleep ( )

Put every added source into its lowest reversible power state.

Returns: True if at least one source actually slept

Calls IMUDataSource::sleep() on all sources, including those whose capability was not claimed. Sources that do not implement sleeping report false and are skipped, so a false return only means nothing could be suspended. Sleeping stops each source's periodic measurement; wakeup() powers the hardware back up but does not restart measurement – restart it explicitly on the units you want sampling again.

◆ tareYaw()

void m5::imu::AhrsController::tareYaw ( )

Zero the publicly reported yaw at the current heading.

Samples the active filter's current fused yaw (the same value quaternion()/euler() would derive from it) and stores a world-frame Z-axis rotation of the opposite angle as an internal tare offset. From then on every quaternion()/euler() call composes this offset on top of the filter's raw attitude, so the reported yaw reads ~0 right after this call while roll and pitch are unaffected. The filter's own internal attitude estimate – and, on a 9-axis setup, its alignment to magnetic north – is left untouched; only the publicly reported attitude is shifted. Calling this again re-zeros to whatever heading the filter has reached by then. See clearYawTare() to remove the offset. No-op (tare stays whatever it already was) if no filter has been selected yet.

◆ update() [1/2]

Capability m5::imu::AhrsController::update ( )

Drain every source and fuse newly-arrived samples, measuring dt from the real clock (m5::utility::micros())

Returns: Capability bits that received at least one new sample this call

The very first call after construction only seeds the internal clock (no previous timestamp to compute an elapsed time from) and returns Capability::None without draining anything; every following call computes dt from the elapsed time since the previous call.

◆ update() [2/2]

Capability m5::imu::AhrsController::update ( const float dt )

Drain every source and fuse newly-arrived samples using an explicit dt.

Parameters

dt	Time elapsed since the previous update call, in seconds; divided evenly across however many gyro samples are drained this call (if none is drained, it is carried over to the next call that drains one; the carried-over part is capped at 0.5 s)

Returns: Capability bits that received at least one new sample this call

◆ wakeup()

bool m5::imu::AhrsController::wakeup ( )

Bring every added source back from sleep()

Returns: True if at least one source actually woke

Calls IMUDataSource::wakeup() on all sources and then discards the internal clock state, so the first update() after waking re-seeds its timebase instead of handing the filter one enormous dt spanning the whole sleep. Measurement is not restarted: call startPeriodicMeasurement() on the units that should resume sampling.

◆ writeAutoCalibration()

void m5::imu::AhrsController::writeAutoCalibration	(	const uint8_t	accel,
		const uint8_t	gyro,
		const uint8_t	mag )

Enable/disable continuous background auto-calibration per capability.

Parameters

accel	Accelerometer strength, 0 = off, 1-255 = adjustment speed (higher = faster)
gyro	Gyroscope strength, 0 = off, 1-255 = adjustment speed
mag	Magnetometer strength, 0 = off, 1-255 = adjustment speed

Note: Only the gyroscope is on by default (strength 1); accel and mag start at 0. An uncorrected gyro zero-rate offset is the dominant source of yaw drift and, when it exceeds VQFFilter::writeBiasClip()'s window, also stops VQF from estimating the bias on its own – so the default keeps it in check without any setup. Strength is effectively the per-sample follow rate x255 while the device is still, so 1 already converges with a roughly 1.3s time constant at a 200Hz sample rate.

A stillness/sphere-fit auto-offset algorithm operating on physical-unit samples. Each capability tracks how "still" its recent samples have been; once that stillness score exceeds a threshold, the offset is nudged toward fitting the sample to the expected shape at rest (accel/mag: a sphere of a fixed radius around the offset; gyro: the origin). The resulting offset is subtracted from every subsequent sample of that capability regardless of strength – setting strength to 0 only stops further adjustment, it does not clear a previously-converged offset (see writeCalibration()/clearing via calibration_data_t{}).

◆ writeAxisOrder()

bool m5::imu::AhrsController::writeAxisOrder	(	const IMUDataSource &	src,
		const AxisOrder &	order )

Set the chip-frame-to-body-frame axis remap applied to one source's Accel/Gyro (and Temp) samples.

Parameters

src	Source to configure (must already have been add()ed)
order	Permutation and per-axis sign flip to apply

Returns: False if src was never add()ed to this controller

Applied every update() to whichever of Accel/Gyro/Temp that source provides, right after raw-to-physical conversion and before auto-calibration offsets are subtracted (see writeAutoCalibration()). Does not affect Mag – accel and gyro always share a single die and are therefore always co-oriented, but an auxiliary magnetometer die (e.g. a BMM150 mounted as a BMI270 AUX chip) can be mounted at a different orientation on the very same source; use writeMagAxisOrder() for that. Defaults to the identity transform (AxisPermutation::XYZ, no inversion) for every added source.

◆ writeCalibration()

bool m5::imu::AhrsController::writeCalibration ( const calibration_data_t & data )

Overwrite every capability's auto-calibration offset from a snapshot.

Parameters

data	Snapshot to restore (see readCalibration())

Returns: False (offsets left untouched) if data.version is not the currently supported version

◆ writeMagAxisOrder()

bool m5::imu::AhrsController::writeMagAxisOrder	(	const IMUDataSource &	src,
		const AxisOrder &	order )

Set the chip-frame-to-body-frame axis remap applied to one source's Mag samples.

Parameters

src	Source to configure (must already have been add()ed)
order	Permutation and per-axis sign flip to apply to Mag only

Returns: False if src was never add()ed to this controller

Applied every update() to the Mag capability of whichever source provides it, right after raw-to-physical conversion and before auto-calibration offsets are subtracted (see writeAutoCalibration()) – independent of writeAxisOrder(), which only covers Accel/Gyro/Temp. Defaults to the identity transform (AxisPermutation::XYZ, no inversion) for every added source.

◆ writeMagCalibrationRadius()

void m5::imu::AhrsController::writeMagCalibrationRadius ( const float radius_ut )

Pin the field strength the magnetometer auto-calibration fits its samples to.

Parameters

radius_ut Local geomagnetic field strength in uT; pass 0 (or less) to go back to estimating it from the data, which is the default

The hard-iron fit works by pushing the offset until every sample sits on a sphere of this radius, so the radius has to match the field the device is actually in. Unlike the accelerometer's 1G sphere, that is not a universal constant: the geomagnetic field runs from roughly 25uT to 65uT depending on location, and a mismatched radius mis-scales the calibration rather than correcting it. By default the radius is therefore learned from the samples themselves; set it here when the local value is known (national geomagnetic surveys publish it) to skip the learning transient.

Note: Changing this resets the learned estimate, not the offset itself; clear the offset via writeCalibration() if a previous calibration converged against a different radius.

◆ writeUserAxisOrder()

void m5::imu::AhrsController::writeUserAxisOrder ( const AxisOrder & order )

Set a global user axis remap composed on top of every source's fixed order.

Parameters

order Permutation and per-axis sign flip the caller wants applied to the reported frame

Mirrors M5Unified's IMU_Class::setAxisOrder user layer. writeAxisOrder()/ writeMagAxisOrder() carry the fixed, per-board correction (chip frame to a unified body frame); this order is applied after that, reorienting the whole device output frame, and so it affects Accel, Gyro, Temp and Mag of every source alike. The two are folded together internally (see compose() in axis_order.hpp), so no extra per-sample transform is paid. Defaults to the identity transform. Composition is recomputed here and on every writeAxisOrder()/writeMagAxisOrder()/add().

Classes

Public Member Functions

Detailed Description

Member Function Documentation

◆ add()

◆ begin()

◆ calibrateGyro() [1/2]

◆ calibrateGyro() [2/2]

◆ clearYawTare()

◆ euler()

◆ hasValidMag()

◆ lastMicros()

◆ quaternion()

◆ readAutoCalibration()

◆ readAxisOrder()

◆ readCalibration()

◆ readMagAxisOrder()

◆ readMagCalibrationRadius()

◆ readUserAxisOrder()

◆ selectFilter()

◆ setFilter()

◆ sleep()

◆ tareYaw()

◆ update() [1/2]

◆ update() [2/2]

◆ wakeup()

◆ writeAutoCalibration()

◆ writeAxisOrder()

◆ writeCalibration()

◆ writeMagAxisOrder()

◆ writeMagCalibrationRadius()

◆ writeUserAxisOrder()