m5::imu::FusionFilter Class Reference

Adapter around the vendored x-io Fusion AHRS algorithm (x-io Technologies, MIT license; see src/imu/filter/third_party/fusion) More...

#include <fusion.hpp>

Inheritance diagram for m5::imu::FusionFilter:

Public Member Functions
virtual void	reset () override
	Reset the internal attitude estimate (and any filter state) to identity.
virtual void	update (const float dt, const float accel[3], const float gyro[3], const float *mag) override
	Fuse one IMU sample into the attitude estimate.
virtual void	quaternion (float &w, float &x, float &y, float &z) const override
	Current fused attitude.
bool	writeGain (const float gain)
	Set the AHRS feedback gain (FusionAhrsSettings.gain)
float	readGain () const
	Current feedback gain.
bool	startupInProgress () const
	Whether Fusion's startup ramp is still in progress.
bool	writeQuaternion (const float w, const float x, const float y, const float z)
	Overwrite the current attitude estimate directly.

Detailed Description

Adapter around the vendored x-io Fusion AHRS algorithm (x-io Technologies, MIT license; see src/imu/filter/third_party/fusion)

Fusion's own attitude/feedback algorithm is used as-is (this adapter performs no re-derivation of the underlying math, unlike ComplementaryFilter/MadgwickFilter/ MahonyFilter/EKFFilter). The adapter's job is limited to the AhrsFilterBase <-> Fusion API boundary:

• Units: Fusion's FusionAhrsUpdate()/FusionAhrsUpdateNoMagnetometer() already take gyroscope in dps, accelerometer in G and magnetometer in "any calibrated units" (see FusionAhrs.h), matching this repository's unit contract (filter_base.hpp) exactly – no scaling is applied here.
• Earth axes convention: FusionAhrsSettings.convention is fixed to FusionConventionNwu (North(X)/West(Y)/Up(Z)) in the constructor, which is the only one of Fusion's three conventions (Nwu/Enu/Ned) consistent with this repository's world frame (filter_base.hpp): FusionConventionNed's earth Z axis is "down", which contradicts a static/level unit reading accel = (0,0,+1) G; that leaves Nwu and Enu, both earth-Z-up, differing only in which world axis the horizontal magnetometer reference aligns to (X for Nwu, Y for Enu) – filter_base.hpp's mag table places that reference along world +X at yaw = 0, which is Nwu's "North" (X) axis. (This is also Fusion's own library default, fusionAhrsDefaultSettings.convention.) The Fusion quaternion is read back via FusionAhrsGetQuaternion() component-for-component (w,x,y,z) with no reordering or sign flip: Fusion's own HalfGravity() formula for Nwu/Enu (see FusionAhrs.c) is algebraically identical to the predicted-gravity-in- body-frame expansion this repository's other filters derive from m5::math::Quaternion::rotate()/fromEuler() (see e.g. MahonyFilter::update()), so the two quaternion representations already agree.
• Startup ramp: Fusion always starts (on FusionAhrsInitialise()/FusionAhrsRestart(), i.e. also after reset()) with a high gain (10.0) that ramps down to the configured gain over a fixed 3-second window (STARTUP_PERIOD in FusionAhrs.c) – this is Fusion's own behavior, not something this adapter can tune. It also means FusionAhrsUpdateNoMagnetometer() forces the yaw heading to 0 on every call while that window is open, so gyro-only yaw integration only becomes observable once the filter has been running (real, cumulative dt) for more than 3 seconds; see test/native/test_filter/fusion_test.cpp for how the shared scenarios account for this.

Member Function Documentation

quaternion()

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

overridevirtual

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

Implements m5::imu::AhrsFilterBase.

reset()

void m5::imu::FusionFilter::reset ( )

overridevirtual

Reset the internal attitude estimate (and any filter state) to identity.

Implements m5::imu::AhrsFilterBase.

startupInProgress()

bool m5::imu::FusionFilter::startupInProgress

(

)

const

Whether Fusion's startup ramp is still in progress.

True for the first 3 seconds (of summed update() dt) after construction or reset(), with the default gain; a gain of 0 or of 10 (Fusion's startup gain) or more ends it on the first update(). During that window Fusion corrects with a high gain and, on a no-magnetometer update(), holds the yaw heading at 0, so a rotation made then is not reflected.

update()

void m5::imu::FusionFilter::update ( const float dt, const float accel[3], const float gyro[3], const float * mag )

overridevirtual

Fuse one IMU sample into the attitude estimate.

Parameters

dt	Time elapsed since the previous update(), in seconds
accel	Accelerometer reading (G), body frame, X/Y/Z
gyro	Gyroscope reading (dps), body frame, X/Y/Z
mag	Magnetometer reading (uT), body frame, X/Y/Z; pass nullptr for 6-axis (accel+gyro only) operation

Implements m5::imu::AhrsFilterBase.

writeGain()

bool m5::imu::FusionFilter::writeGain

(

const float

gain

)

Set the AHRS feedback gain (FusionAhrsSettings.gain)

Parameters

gain	Feedback gain, must be >= 0 (0 disables accelerometer/magnetometer outlier rejection as well as feedback correction speed limiting – see Fusion's own FusionAhrsSetSettings())

Returns

False (and keeps the previous value) if gain is negative

writeQuaternion()

bool m5::imu::FusionFilter::writeQuaternion	(	const float	w,
		const float	x,
		const float	y,
		const float	z )

Overwrite the current attitude estimate directly.

Not part of AhrsFilterBase; intended for seeding a known starting attitude (e.g. tests, or resuming from a previously stored orientation).

Parameters

w	W component of the quaternion
x	X component of the quaternion
y	Y component of the quaternion
z	Z component of the quaternion

Returns

False (and keeps the previous value) if the resulting quaternion is (near) zero